GT2005: Tools/Actorics/RobotVertices.cpp Source File

00001 /**
00002 * @file RobotVertices.cpp
00003 *
00004 * Implementation of struct RobotVertices
00005 * 
00006 * @author Uwe Düffert
00007 */
00008 
00009 #include "RobotVertices.h"
00010 #include "Tools/Math/Common.h"
00011 
00012 RobotVertices::RobotVertices(const RobotDimensions& r):bodyTilt(0),bodyRoll(0),neckHeight(0),frameNumber(0)
00013 {
00014   neck=Vector3<double>(0,0,r.lowerForeLegLength+r.upperLegLength+r.neckToLegsLengthZ);
00015 
00016   //order is the same as in Kinematics::LegIndex{fr,fl,hr,hl}:
00017   footPosition[0]=Vector3<double>(-r.neckToLegsLengthX,-r.bodyWidth/2,0);
00018   footPosition[1]=Vector3<double>(-r.neckToLegsLengthX,r.bodyWidth/2,0);
00019   footPosition[2]=Vector3<double>(-r.neckToLegsLengthX-r.lengthBetweenLegs,-r.bodyWidth/2,0);
00020   footPosition[3]=Vector3<double>(-r.neckToLegsLengthX-r.lengthBetweenLegs,r.bodyWidth/2,0);
00021 
00022   kneePosition[0]=footPosition[0]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroFrontKneeArc-r.zeroShoulderArc)*r.lowerForeLegLength);
00023   kneePosition[1]=footPosition[1]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroFrontKneeArc-r.zeroShoulderArc)*r.lowerForeLegLength);
00024   kneePosition[2]=footPosition[2]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroHindKneeArc-r.zeroShoulderArc)*r.lowerHindLegLength);
00025   kneePosition[3]=footPosition[3]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroHindKneeArc-r.zeroShoulderArc)*r.lowerHindLegLength);
00026 
00027   shoulderPosition[0]=kneePosition[0]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00028   shoulderPosition[1]=kneePosition[1]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00029   shoulderPosition[2]=kneePosition[2]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00030   shoulderPosition[3]=kneePosition[3]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00031 }
00032 
00033 In& operator>>(In& stream,RobotVertices& robotVertices)
00034 {
00035   stream.read(&robotVertices,sizeof(robotVertices));
00036   return stream;
00037 }
00038  
00039 Out& operator<<(Out& stream, const RobotVertices& robotVertices)
00040 {
00041   stream.write(&robotVertices,sizeof(robotVertices));
00042   return stream;
00043 }