protected internal override void OnApplyConstraint(UserJointBase joint)
{
Matrix3D m = Joint.BodyToWorldMatrix(joint.ParentBody, _parentDirection);
Matrix3D c = Joint.BodyToWorldMatrix(joint.ChildBody, this.Direction);
double sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m)), Math3D.GetFrontVector(ref c));
double ca = Vector3D.DotProduct(Math3D.GetUpVector(ref c), Math3D.GetUpVector(ref m));
double a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetFrontVector(ref m));
ApplyConstraintProperties(joint);
sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m)), Math3D.GetUpVector(ref c));
ca = Vector3D.DotProduct(Math3D.GetRightVector(ref c), Math3D.GetRightVector(ref m));
a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetUpVector(ref m));
ApplyConstraintProperties(joint);
sa = Vector3D.DotProduct(Vector3D.CrossProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m)), Math3D.GetRightVector(ref c));
ca = Vector3D.DotProduct(Math3D.GetFrontVector(ref c), Math3D.GetFrontVector(ref m));
a = Math.Atan2(sa, ca);
joint.NewtonJoint.UserBilateralAddAngularRow((float)a, Math3D.GetRightVector(ref m));
ApplyConstraintProperties(joint);
}
protected internal override void OnApplyConstraint(UserJointBase joint)
{
Matrix3D m = Joint.BodyToWorldMatrix(joint.ChildBody, this.PivotPoint, this.Direction);
Vector3D childPivotPoint = Math3D.GetOffset(ref m);
Vector3D parentPivotPoint = (Vector3D)Joint.BodyToWorld(joint.ParentBody, _parentPivotPoint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetFrontVector(ref m)
);
ApplyConstraintProperties(joint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetUpVector(ref m)
);
ApplyConstraintProperties(joint);
joint.NewtonJoint.UserBilateralAddLinearRow(
childPivotPoint, parentPivotPoint,
Math3D.GetRightVector(ref m)
);
ApplyConstraintProperties(joint);
}
