Pose3D ViewBikeMath::calculateHandPos(const JointData& jointData, const JointData::Joint& joint, const RobotDimensions& robotDimensions)
{
Pose3D handPos;
float sign = joint == JointData::LShoulderPitch ? 1.f : -1.f;
handPos.translate(robotDimensions.armOffset.x, sign * robotDimensions.armOffset.y, robotDimensions.armOffset.z);
handPos.rotateY(-jointData.angles[joint]);
handPos.rotateZ(sign * jointData.angles[joint + 1]);
handPos.translate(robotDimensions.upperArmLength, 0, 0);
handPos.rotateX(jointData.angles[joint + 2] * sign);
handPos.rotateZ(sign * jointData.angles[joint + 3]);
return handPos;
}
bool Transformation::robotWithCameraRotationToImage(const Vector2<>& point,
const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo,
Vector2<>& pointInImage)
{
Pose3D cameraRotatedMatrix;
cameraRotatedMatrix.rotateZ(cameraMatrix.rotation.getZAngle());
const Vector3<> point3D(point.x, point.y, 0.f);
return robotToImage(cameraRotatedMatrix * point3D, cameraMatrix, cameraInfo, pointInImage);
}
bool Transformation::imageToRobotWithCameraRotation(const Vector2<int>& pointInImage,
const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo,
Vector2<>& relativePosition)
{
const bool ret = imageToRobot(pointInImage, cameraMatrix, cameraInfo, relativePosition);
if(ret)
{
Pose3D cameraRotatedMatrix;
cameraRotatedMatrix.rotateZ(cameraMatrix.rotation.getZAngle());
const Vector3<> point3D = cameraRotatedMatrix.invert() * Vector3<>(relativePosition.x, relativePosition.y, 0.f);
relativePosition.x = point3D.x;
relativePosition.y = point3D.y;
}
return ret;
}
