private static void AddDamping(Simulation simulation, RigidBody bodyA, RigidBody bodyB, float softness, float maxForce)
{
// Usually an AngularVelocityMotor rotates bodies, but in this case the
// TargetVelocity is set to 0 (default). The AngularVelocityMotor acts as
// a damping.
AngularVelocityMotor damping = new AngularVelocityMotor
{
BodyA = bodyA,
BodyB = bodyB,
// A softness of 0 would make the joint stiff. A softness value > 0 is important to
// mimic damping.
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(damping);
}
public ConstraintSample3(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- HingeJoint with AngularVelocityMotor
// A board is fixed on a pole like a wind wheel.
// An AngularVelocityMotor creates a rotation with constant angular velocity around the
// hinge axis.
RigidBody box0 = new RigidBody(new BoxShape(0.1f, 2f, 0.1f))
{
Pose = new Pose(new Vector3F(-2, 1, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box0);
RigidBody box1 = new RigidBody(new BoxShape(0.1f, 0.4f, 1f))
{
Pose = new Pose(new Vector3F(-2 + 0.05f, 1.8f, 0))
};
Simulation.RigidBodies.Add(box1);
HingeJoint hingeJoint = new HingeJoint
{
BodyA = box0,
BodyB = box1,
AnchorPoseALocal = new Pose(new Vector3F(0.05f, 0.8f, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(-0.05f, 0, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(hingeJoint);
AngularVelocityMotor angularVelocityMotor = new AngularVelocityMotor
{
BodyA = box0,
// The rotation axis is the local x axis of BodyA.
AxisALocal = Vector3F.UnitX,
BodyB = box1,
TargetVelocity = 10,
// The motor power is limit, so that the rotation can be stopped by other objects blocking
// the movement.
MaxForce = 10000,
// The HingeJoint controls all other axes. So this motor must only act on the hinge
// axis.
UseSingleAxisMode = true,
CollisionEnabled = false,
};
Simulation.Constraints.Add(angularVelocityMotor);
// ----- A PrismaticJoint with a LinearVelocityMotor.
RigidBody box2 = new RigidBody(new BoxShape(0.7f, 0.7f, 0.7f))
{
Pose = new Pose(new Vector3F(0, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box2);
RigidBody box3 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3F(0, 1, 0))
};
Simulation.RigidBodies.Add(box3);
_prismaticJoint = new PrismaticJoint
{
BodyA = box2,
BodyB = box3,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_prismaticJoint);
_linearVelocityMotor = new LinearVelocityMotor
{
BodyA = box2,
AxisALocal = -Vector3F.UnitY,
BodyB = box3,
TargetVelocity = 1,
CollisionEnabled = false,
MaxForce = 10000,
UseSingleAxisMode = true,
};
Simulation.Constraints.Add(_linearVelocityMotor);
// ----- A BallJoint with a TwistSwingLimit and a QuaternionMotor
// The ball joint connects a cylinder to a static box.
// The twist-swing limits rotational movement.
// The quaternion motor acts like a spring that controls the angle of joint.
RigidBody box4 = new RigidBody(new BoxShape(0.5f, 0.5f, 0.5f))
{
Pose = new Pose(new Vector3F(2, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box4);
RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 0.75f))
{
Pose = new Pose(new Vector3F(2, 2 - 0.75f / 2 - 0.25f, 0))
};
Simulation.RigidBodies.Add(cylinder0);
_ballJoint = new BallJoint
{
BodyA = box4,
BodyB = cylinder0,
AnchorPositionALocal = new Vector3F(0, -0.25f, 0),
AnchorPositionBLocal = new Vector3F(0, 0.75f / 2, 0),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_ballJoint);
_twistSwingLimit = new TwistSwingLimit
{
BodyA = box4,
// The first column is the twist axis (-y). The other two columns are the swing axes.
AnchorOrientationALocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
BodyB = cylinder0,
AnchorOrientationBLocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
CollisionEnabled = false,
// The twist is limited to +/- 10°. The swing limits are +/- 40° and +/- 60°. This creates
// a deformed cone that limits the swing movements (see visualization).
Minimum = new Vector3F(-MathHelper.ToRadians(10), -MathHelper.ToRadians(40), -MathHelper.ToRadians(60)),
Maximum = new Vector3F(MathHelper.ToRadians(10), MathHelper.ToRadians(40), MathHelper.ToRadians(60)),
};
Simulation.Constraints.Add(_twistSwingLimit);
QuaternionMotor quaternionMotor = new QuaternionMotor
{
BodyA = box4,
AnchorOrientationALocal = Matrix33F.Identity,
BodyB = cylinder0,
AnchorOrientationBLocal = Matrix33F.Identity,
CollisionEnabled = false,
// The QuaternionMotor controls the orientation of the second body relative to the first
// body. Here, we define that the cylinder should swing 30° away from the default
// orientation.
TargetOrientation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(30)),
// Position and orientation motors are similar to damped-springs. We can control
// the stiffness and damping of the spring. (It is also possible to set the SpringConstant
// to 0 if the QuaternionMotor should only act as a rotational damping.)
SpringConstant = 100,
DampingConstant = 20,
};
Simulation.Constraints.Add(quaternionMotor);
}
public ConstraintCarSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Create a material with high friction - this will be used for the ground and the wheels
// to give the wheels some traction.
UniformMaterial roughMaterial = new UniformMaterial
{
DynamicFriction = 1,
StaticFriction = 1,
};
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0), null, roughMaterial)
{
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// Now, we build a car out of one box for the chassis and 4 cylindric wheels.
// Front wheels are fixed with Hinge2Joints and motorized with AngularVelocityMotors.
// Back wheels are fixed with HingeJoints and not motorized. - This creates a sloppy
// car configuration. Please note that cars for racing games are not normally built with
// simple constraints - nevertheless, it is funny to do and play with it.
// Check out the "Vehicle Sample" (not included in this project)! The "Vehicle Sample"
// provides a robust ray-car implementation.)
// ----- Chassis
BoxShape chassisShape = new BoxShape(1.7f, 1, 4f);
MassFrame chassisMass = MassFrame.FromShapeAndDensity(chassisShape, Vector3F.One, 200, 0.01f, 3);
// Here is a trick: The car topples over very easily. By lowering the center of mass we
// make it more stable.
chassisMass.Pose = new Pose(new Vector3F(0, -1, 0));
RigidBody chassis = new RigidBody(chassisShape, chassisMass, null)
{
Pose = new Pose(new Vector3F(0, 1, 0)),
};
Simulation.RigidBodies.Add(chassis);
// ------ Wheels
CylinderShape cylinderShape = new CylinderShape(0.4f, 0.3f);
MassFrame wheelMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3F.One, 500, 0.01f, 3);
RigidBody wheelFrontLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontLeft);
RigidBody wheelFrontRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontRight);
RigidBody wheelBackLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackLeft);
RigidBody wheelBackRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackRight);
// ----- Hinge2Joints for the front wheels.
// A Hinge2Joint allows a limited rotation on the first constraint axis - the steering
// axis. The second constraint axis is locked and the third constraint axis is the
// rotation axis of the wheels.
_frontLeftHinge = new Hinge2Joint
{
BodyA = chassis,
// --> To define the constraint anchor orientation for the chassis:
// The columns are the axes. We set the local y axis in the first column. This is
// steering axis. In the last column we set the -x axis. This is the wheel rotation axis.
// In the middle column is a vector that is normal to the first and last axis.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, -1.4f),
new Matrix33F(0, 0, -1,
1, 0, 0,
0, -1, 0)),
BodyB = wheelFrontLeft,
// --> To define the constraint anchor orientation for the chassis:
// The columns are the axes. We set the local x axis in the first column. This is
// steering axis. In the last column we set the y axis. This is the wheel rotation axis.
// (In local space of a cylinder the cylinder axis is the +y axis.)
// In the middle column is a vector that is normal to the first and last axis.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0,
0, 0, 1,
0, -1, 0)),
CollisionEnabled = false,
Minimum = new Vector2F(-0.7f, float.NegativeInfinity),
Maximum = new Vector2F(0.7f, float.PositiveInfinity),
};
Simulation.Constraints.Add(_frontLeftHinge);
_frontRightHinge = new Hinge2Joint
{
BodyA = chassis,
BodyB = wheelFrontRight,
AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, -1.4f),
new Matrix33F(0, 0, -1,
1, 0, 0,
0, -1, 0)),
AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0,
0, 0, 1,
0, -1, 0)),
CollisionEnabled = false,
Minimum = new Vector2F(-0.7f, float.NegativeInfinity),
Maximum = new Vector2F(0.7f, float.PositiveInfinity),
};
Simulation.Constraints.Add(_frontRightHinge);
// ----- HingeJoints for the back wheels.
// Hinges allow free rotation on the first constraint axis.
HingeJoint backLeftHinge = new HingeJoint
{
BodyA = chassis,
AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, 1.4f)),
BodyB = wheelBackLeft,
// --> To define the constraint anchor orientation:
// The columns are the axes. We set the local y axis in the first column. This is
// cylinder axis and should be the hinge axis. In the other two columns we set two
// orthonormal vectors.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1,
1, 0, 0,
0, 1, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(backLeftHinge);
HingeJoint backRightHinge = new HingeJoint
{
BodyA = chassis,
AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, 1.4f)),
BodyB = wheelBackRight,
AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1,
1, 0, 0,
0, 1, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(backRightHinge);
// ----- Motors for the front wheels.
// (Motor axes and target velocities are set in Update() below.)
_frontLeftMotor = new AngularVelocityMotor
{
BodyA = chassis,
BodyB = wheelFrontLeft,
CollisionEnabled = false,
// We use "single axis mode", which means the motor drives only on axis and does not
// block motion orthogonal to this axis. - Rotation about the orthogonal axes is already
// controlled by the Hinge2Joint.
UseSingleAxisMode = true,
// The motor has only limited power:
MaxForce = 50000,
};
Simulation.Constraints.Add(_frontLeftMotor);
_frontRightMotor = new AngularVelocityMotor
{
BodyA = chassis,
BodyB = wheelFrontRight,
CollisionEnabled = false,
UseSingleAxisMode = true,
MaxForce = 50000,
};
Simulation.Constraints.Add(_frontRightMotor);
// ----- Drop a few boxes to create obstacles.
BoxShape boxShape = new BoxShape(1, 1, 1);
MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 100, 0.01f, 3);
for (int i = 0; i < 20; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-20, 20);
position.Y = 5;
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
RigidBody body = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(body);
}
}
