private ICollisionJoint[] GetCarConstraints(List <ICollisionShape> shape)
{
ICollisionJoint[] constraints = new ICollisionJoint[4];
//Wheels Joints
constraints[0] = new Hinge2Joint(
shape[1],
shape[2],
shape[3],
new Vector3d(-1.1, -0.5, -1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[0].SetAxis1AngularLimit(0.0, 0.0);
constraints[1] = new Hinge2Joint(
shape[1],
shape[4],
shape[5],
new Vector3d(1.1, -0.5, 1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[1].SetAxis1AngularLimit(-0.78539816339, 0.78539816339);
constraints[2] = new Hinge2Joint(
shape[1],
shape[3],
shape[2],
new Vector3d(1.1, -0.5, -1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[2].SetAxis1AngularLimit(0.0, 0.0);
constraints[3] = new Hinge2Joint(
shape[1],
shape[5],
shape[4],
new Vector3d(-1.1, -0.5, 1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[3].SetAxis1AngularLimit(-0.78539816339, 0.78539816339);
return(constraints);
}
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(Vector3.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, Vector3.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 Vector3(0, -1, 0));
RigidBody chassis = new RigidBody(chassisShape, chassisMass, null)
{
Pose = new Pose(new Vector3(0, 1, 0)),
};
Simulation.RigidBodies.Add(chassis);
// ------ Wheels
CylinderShape cylinderShape = new CylinderShape(0.4f, 0.3f);
MassFrame wheelMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3.One, 500, 0.01f, 3);
RigidBody wheelFrontLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontLeft);
RigidBody wheelFrontRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelFrontRight);
RigidBody wheelBackLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(wheelBackLeft);
RigidBody wheelBackRight = new RigidBody(cylinderShape, wheelMass, roughMaterial)
{
Pose = new Pose(new Vector3(0, 1, 0), Matrix.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 Vector3(-0.9f, -0.4f, -1.4f),
new Matrix(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 Matrix(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 Vector3(0.9f, -0.4f, -1.4f),
new Matrix(0, 0, -1,
1, 0, 0,
0, -1, 0)),
AnchorPoseBLocal = new Pose(new Matrix(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 Vector3(-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 Matrix(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 Vector3(0.9f, -0.4f, 1.4f)),
BodyB = wheelBackRight,
AnchorPoseBLocal = new Pose(new Matrix(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, Vector3.One, 100, 0.01f, 3);
for (int i = 0; i < 20; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-20, 20);
position.Y = 5;
Quaternion orientation = RandomHelper.Random.NextQuaternion();
RigidBody body = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(body);
}
}
public ICollisionJoint[] LoadSimulationJoints(
ICollisionShape[] objects)
{
var xmlDoc = new XmlDocument();
xmlDoc.Load(FileNameObjectProperties);
XmlNodeList xmlList = xmlDoc.SelectNodes(nodePathJoints);
ICollisionJoint[] joints = new ICollisionJoint[xmlList.Count];
for (int i = 0; i < xmlList.Count; i++)
{
//Object index A
int indexA = Convert.ToInt32(xmlList[i][objectIndexAAttribute].InnerText);
//Object index B
int indexB = Convert.ToInt32(xmlList[i][objectIndexBAttribute].InnerText);
XmlNodeList jointPropertiesList = xmlList[i].SelectNodes(jointProperties);
ICollisionJoint[] joint = new ICollisionJoint[jointPropertiesList.Count];
for (int j = 0; j < jointPropertiesList.Count; j++)
{
//Joint type
var jointType = (JointType)Convert.ToInt32(jointPropertiesList[j][this.jointType].InnerText);
//Restore coefficient
double K = Convert.ToDouble(jointPropertiesList[j][restoreCoeffAttribute].InnerText);
//Stretch coefficient
double C = Convert.ToDouble(jointPropertiesList[j][stretchCoeffAttribute].InnerText);
//Position
var startAnchorPosition = new Vector3d(
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["x"].Value),
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["y"].Value),
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["z"].Value));
//Action Axis
var actionAxis = new Vector3d(
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["x"].Value),
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["y"].Value),
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["z"].Value));
switch (jointType)
{
case JointType.Fixed:
joint [j] = new FixedJoint(
objects[indexA],
objects[indexB],
K,
C);
break;
case JointType.BallAndSocket:
joint[j] = new BallAndSocketJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
K,
C);
break;
case JointType.Slider:
joint[j] = new SliderJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetLinearLimit(Convert.ToDouble(jointPropertiesList[j][linearLimitMin].InnerText), Convert.ToDouble(jointPropertiesList[j][linearLimitMax].InnerText));
break;
case JointType.Piston:
joint[j] = new PistonJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetLinearLimit(
Convert.ToDouble(jointPropertiesList[j][linearLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][linearLimitMax].InnerText));
break;
case JointType.Hinge:
joint[j] = new HingeJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetAxis1Motor(3.0, 0.15);
break;
case JointType.Universal:
joint[j] = new UniversalJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
new Vector3d(1.0, 0.0, 0.0),
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetAxis2AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
break;
case JointType.Hinge2:
joint[j] = new Hinge2Joint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
new Vector3d(1.0, 0.0, 0.0),
K,
1.0,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
//joint[j].SetAxis2Motor(4.0, 3.0);
break;
case JointType.Angular:
joint[j] = new AngularJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
new Vector3d(1.0, 0.0, 0.0),
new Vector3d(0.0, 1.0, 0.0),
0.16,
0.008,
0.008);
break;
}
joints[i] = joint[j];
}
}
return(joints);
}
