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);
}
internal float Evaluate(XForm transformA, XForm transformB)
{
switch (FaceType)
{
case Type.Points:
{
Vec2 axisA = Common.Math.MulT(transformA.R, Axis);
Vec2 axisB = Common.Math.MulT(transformB.R, -Axis);
Vec2 localPointA = ShapeA.GetSupportVertex(axisA);
Vec2 localPointB = ShapeB.GetSupportVertex(axisB);
Vec2 pointA = Common.Math.Mul(transformA, localPointA);
Vec2 pointB = Common.Math.Mul(transformB, localPointB);
float separation = Vec2.Dot(pointB - pointA, Axis);
return(separation);
}
case Type.FaceA:
{
Vec2 normal = Common.Math.Mul(transformA.R, Axis);
Vec2 pointA = Common.Math.Mul(transformA, LocalPoint);
Vec2 axisB = Common.Math.MulT(transformB.R, -normal);
Vec2 localPointB = ShapeB.GetSupportVertex(axisB);
Vec2 pointB = Common.Math.Mul(transformB, localPointB);
float separation = Vec2.Dot(pointB - pointA, normal);
return(separation);
}
case Type.FaceB:
{
Vec2 normal = Common.Math.Mul(transformB.R, Axis);
Vec2 pointB = Common.Math.Mul(transformB, LocalPoint);
Vec2 axisA = Common.Math.MulT(transformA.R, -normal);
Vec2 localPointA = ShapeA.GetSupportVertex(axisA);
Vec2 pointA = Common.Math.Mul(transformA, localPointA);
float separation = Vec2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Box2DNetDebug.Assert(false);
return(0.0f);
}
}
internal float Evaluate(Transform TransformA, Transform TransformB)
{
switch (FaceType)
{
case Type.Points:
{
Vector2 axisA = TransformA.InverseTransformDirection(Axis);
Vector2 axisB = TransformB.InverseTransformDirection(-Axis);
Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
Vector2 pointA = TransformA.TransformPoint(localPointA);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float separation = Vector2.Dot(pointB - pointA, Axis);
return(separation);
}
case Type.FaceA:
{
Vector2 normal = TransformA.TransformDirection(Axis);
Vector2 pointA = TransformA.TransformPoint(LocalPoint);
Vector2 axisB = TransformB.InverseTransformDirection(-normal);
Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case Type.FaceB:
{
Vector2 normal = TransformB.TransformDirection(Axis);
Vector2 pointB = TransformB.TransformPoint(LocalPoint);
Vector2 axisA = TransformA.InverseTransformDirection(-normal);
Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
Vector2 pointA = TransformA.TransformPoint(localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Box2DNetDebug.Assert(false);
return(0.0f);
}
}
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);
}
}
public static void Paint(this ShapeB shape)
{
// Your paint code
}
public Vector2 Support(Vector2 direction)
{
return(ShapeA.Support(direction, TransformA) - ShapeB.Support(-direction, TransformB));
}
