public Vector3 TransRobot2LeftArm(Vector3 vector)
{
HomogeneousM leftArm2neck = new HomogeneousM(neck2LeftArmMatrix().Inverse);
HomogeneousM neck2robot = new HomogeneousM(robot2neckMatrix().Inverse);
HomogeneousM arm2robot = new HomogeneousM(leftArm2neck.Matrix * neck2robot.Matrix);
return(arm2robot.Transform(vector));
}
public Vector3 TransHeadKinect2Robot(Vector3 vector)
{
HomogeneousM robot2HeadkinectMatrix = new HomogeneousM(robot2neckMatrix().Matrix *neck2headKinectMatrix().Matrix);
return(robot2HeadkinectMatrix.Transform(vector));
}
public Vector3 TransLeftArm2Robot(Vector3 vector)
{
HomogeneousM robot2leftArm = new HomogeneousM(robot2neckMatrix().Matrix *neck2LeftArmMatrix().Matrix);
return(robot2leftArm.Transform(vector));
}
public Vector3 TransMinoru2Robot(Vector3 vector)
{
HomogeneousM robot2minoruMatrix = new HomogeneousM(robot2neckMatrix().Matrix *neck2minoruMatrix().Matrix);
return(new Vector3(robot2minoruMatrix.Transform(vector)));
}
public Vector3 TransChestKinect2Robot(Vector3 vector)
{
HomogeneousM chestKinect2robot = new HomogeneousM(robot2neckMatrix().Matrix *neck2chestKinectMatrix().Matrix);
return(new Vector3(chestKinect2robot.Transform(vector)));
}
public Vector3 TransRobot2Neck(Vector3 vector)
{
HomogeneousM neck2robotMatrix = new HomogeneousM(robot2neckMatrix().Inverse);
return(new Vector3(neck2robotMatrix.Transform(vector)));
}
