void RMSSystem::Translate(const VECTOR3 &dPos, VECTOR3& newPos)
{
if(RMSMode[5].IsSet()) { // END EFF
// Reference Frame:
// X: in direction of EE Z: opposite to camera direction Y: completes RH frame
VECTOR3 y_axis = crossp(arm_ik_rot, arm_ik_dir);
// create rotation matrix to convert vector from EE frame to Orbiter body frame
MATRIX3 EERotMatrix = _M(arm_ik_dir.x, y_axis.x, arm_ik_rot.x,
arm_ik_dir.y, y_axis.y, arm_ik_rot.y,
arm_ik_dir.z, y_axis.z, arm_ik_rot.z);
// matrix to convert vector from EE frame to IK frame
MATRIX3 IKRotMatrix = mul(EERotMatrix, Transpose(GetRotationMatrix(_V(1, 0, 0), RMS_Z_AXIS_ANGLE)));
newPos = arm_ik_pos+mul(IKRotMatrix, dPos);
}
else if(RMSMode[6].IsSet()) { // ORB LD
//MoveEE(arm_ee_pos+RotateVectorX(dPos, -RMS_ROLLOUT_ANGLE), arm_ee_dir, arm_ee_rot);
newPos = arm_ik_pos+RotateVectorX(dPos, RMS_ROLLOUT_ANGLE);
}
}