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)); }