public void AddJoint(Physics3DJoint joint, Physics3DRigidBody r1, Physics3DRigidBody r2, Components.JointDesc desc)
{
var t0 = MatrixExt.Transform(desc.Position, ToQuaternion(desc.Rotation));
Matrix4x4.Invert(t0, out var res);
Matrix4x4.Invert(MatrixExt.Transform(r1.defaultPosition, r1.defaultRotation), out var t1);
Matrix4x4.Invert(MatrixExt.Transform(r2.defaultPosition, r2.defaultRotation), out var t2);
t1 = t0 * t1;
t2 = t0 * t2;
var j = new Generic6DofSpringConstraint(r1.rigidBody, r2.rigidBody, GetMatrix(t1), GetMatrix(t2), true);
joint.constraint = j;
j.LinearLowerLimit = GetVector3(desc.PositionMinimum);
j.LinearUpperLimit = GetVector3(desc.PositionMaximum);
j.AngularLowerLimit = GetVector3(desc.RotationMinimum);
j.AngularUpperLimit = GetVector3(desc.RotationMaximum);
S(0, desc.PositionSpring.X);
S(1, desc.PositionSpring.Y);
S(2, desc.PositionSpring.Z);
S(3, desc.RotationSpring.X);
S(4, desc.RotationSpring.Y);
S(5, desc.RotationSpring.Z);
void S(int index, float f)
{
if (f != 0.0f)
{
j.EnableSpring(index, true);
j.SetStiffness(index, f);
}
else
{
j.EnableSpring(index, false);
}
}
world.AddConstraint(joint.constraint);
}
public void RemoveJoint(Physics3DJoint joint)
{
world.RemoveConstraint(joint.constraint);
joint.constraint.Dispose();
}
