Ejemplo n.º 1
0
        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);
        }
Ejemplo n.º 2
0
 public void RemoveJoint(Physics3DJoint joint)
 {
     world.RemoveConstraint(joint.constraint);
     joint.constraint.Dispose();
 }