public override void AddTorque(double torqueAxis1, double torqueAxis2)
{
Vector3d hingeAxis = ShapeA.RotationMatrix * HingeAxis;
Vector3d torque = hingeAxis * torqueAxis1;
ShapeA.SetTorque(ShapeA.TorqueValue + torque);
ShapeB.SetTorque(ShapeB.TorqueValue - torque);
}
public void AddTorqueShapeB(double torqueAxis1, double torqueAxis2)
{
Vector3d hingeAxis = GetHingeAxis();
Vector3d rotationAxis = GetRotationAxis();
Vector3d torque = rotationAxis * torqueAxis2 + hingeAxis * torqueAxis1;
ShapeB.SetTorque(ShapeB.TorqueValue + torque);
}