public void RotateHingeAxis(double angle)
{
Vector3d hingeAxis = GetHingeAxis();
Vector3d rotationAxis = GetRotationAxis();
var rotationQuaternion = new Quaternion(hingeAxis, angle);
var rt = (rotationQuaternion * ShapeB.RotationStatus).Normalize();
var bufRotationAxis = rt.ConvertToMatrix() * RotationAxis;
double angle1 = GetAngle1(
hingeAxis,
bufRotationAxis,
HingeAxis,
ShapeA.RotationStatus,
RelativeOrientation1);
if (angle1 > AngularLimitMin1.Value &&
angle1 < AngularLimitMax1.Value)
{
ShapeB.Rotate(hingeAxis, angle);
}
else if (angle1 < AngularLimitMin1.Value)
{
angle1 = GetAngle1(
hingeAxis,
rotationAxis,
HingeAxis,
ShapeA.RotationStatus,
RelativeOrientation1);
double anglediff = AngularLimitMin1.Value - angle1;
ShapeB.Rotate(hingeAxis, anglediff);
}
else if (angle1 > AngularLimitMax1.Value)
{
angle1 = GetAngle1(
hingeAxis,
rotationAxis,
HingeAxis,
ShapeA.RotationStatus,
RelativeOrientation1);
double anglediff = AngularLimitMax1.Value - angle1;
ShapeB.Rotate(hingeAxis, anglediff);
}
}
