/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public TwistTestDemo(DemosGame game)
: base(game)
{
var a = new Box(new Vector3(-2, 2, 0), 1, 2, 2, 5);
var b = new Box(new Vector3(2, 2, 0), 1, 2, 2, 5);
b.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.PiOver4);
Space.Add(a);
Space.Add(b);
var twistJoint = new TwistJoint(a, b, a.OrientationMatrix.Right, b.OrientationMatrix.Right);
var twistMotor = new TwistMotor(a, b, a.OrientationMatrix.Right, b.OrientationMatrix.Right);
twistMotor.Settings.Mode = MotorMode.Servomechanism;
//Space.Add(twistJoint);
Space.Add(twistMotor);
var ballSocketJoint = new BallSocketJoint(a, b, (a.Position + b.Position) * 0.5f);
var swingLimit = new SwingLimit(a, b, a.OrientationMatrix.Right, a.OrientationMatrix.Right, MathHelper.PiOver2);
Space.Add(ballSocketJoint);
Space.Add(swingLimit);
Box ground = new Box(new Vector3(0, -.5f, 0), 50, 1, 50);
Space.Add(ground);
game.Camera.Position = new Vector3(0, 6, 15);
}
/// <summary>
/// Constructs a new constraint which restricts the linear and angular motion between two entities.
/// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
/// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
/// </summary>
public WeldJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
NoRotationJoint = new NoRotationJoint();
Add(BallSocketJoint);
Add(NoRotationJoint);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="freeAxis">Axis around which the hinge can rotate.</param>
public RevoluteJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 freeAxis)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
Limit = new RevoluteLimit(connectionA, connectionB);
Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
Limit.IsActive = false;
Motor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
Vector3 baseAxis = anchor - connectionA.position;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
{
baseAxis = connectionB.position - anchor;
}
baseAxis -= Vector3.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = Vector3.Cross(freeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(freeAxis, Vector3.Right);
}
}
Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
Motor.Basis.SetWorldAxes(freeAxis, baseAxis, Vector3.Cross(freeAxis, baseAxis), connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = Vector3.Cross(freeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(freeAxis, Vector3.Right);
}
}
Limit.TestAxis = baseAxis;
Motor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
/// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
/// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
/// </summary>
public UniversalJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
TwistJoint = new TwistJoint();
Limit = new TwistLimit();
Motor = new TwistMotor();
Add(BallSocketJoint);
Add(TwistJoint);
Add(Limit);
Add(Motor);
}
public PointPointConstraint(Vector3 position, IPhysicObject obA, IPhysicObject obB)
{
if (obA.PhysicObjectTypes != PhysicObjectTypes.TRIANGLEMESHOBJECT && obB.PhysicObjectTypes != PhysicObjectTypes.TRIANGLEMESHOBJECT)
{
BodyA = (BepuEntityObject)obA;
BodyB = (BepuEntityObject)obB;
joint = new BallSocketJoint(BodyA.Entity, BodyB.Entity, position);
}
else
{
throw new Exception("Cannot apply this Constraint on Triangle Meshes");
}
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
/// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
/// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
/// </summary>
public RevoluteJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
AngularJoint = new RevoluteAngularJoint();
Limit = new RevoluteLimit();
Motor = new RevoluteMotor();
Add(BallSocketJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts the linear and angular motion between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">The location of the weld.</param>
public WeldJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
NoRotationJoint = new NoRotationJoint(connectionA, connectionB);
Add(BallSocketJoint);
Add(NoRotationJoint);
}
/// <summary>
/// Constructs a new constraint which restricts the linear and angular motion between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
public WeldJoint(Entity connectionA, Entity connectionB)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, (connectionA.position + connectionB.position) * .5f);
NoRotationJoint = new NoRotationJoint(connectionA, connectionB);
Add(BallSocketJoint);
Add(NoRotationJoint);
}
void jointDemo()
{
BepuEntity e1;
BepuEntity e2;
Joint joint;
// Ball & socket joint
e1 = createBox(new Vector3(20, 5, -20), 1, 1, 5);
e1.body.BecomeKinematic();
e2 = createBox(new Vector3(20, 5, -10), 1, 1, 5);
joint = new BallSocketJoint(e1.body, e2.body, new Vector3(20, 5, -15));
space.Add(joint);
// Hinge
e1 = createBox(new Vector3(30, 5, -20), 1, 1, 5);
e1.body.BecomeKinematic();
e2 = createBox(new Vector3(30, 5, -10), 1, 1, 5);
RevoluteJoint hinge = new RevoluteJoint(e1.body, e2.body, new Vector3(20, 5, -15), new Vector3(1, 0, 0));
space.Add(hinge);
// Universal
e1 = createBox(new Vector3(40, 5, -20), 1, 1, 5);
e2 = createBox(new Vector3(40, 5, -10), 1, 1, 5);
UniversalJoint uni = new UniversalJoint(e1.body, e2.body, new Vector3(40, 5, -15));
space.Add(uni);
// Weld Joint
e1 = createBox(new Vector3(50, 5, -20), 1, 1, 5);
e2 = createBox(new Vector3(50, 5, -10), 1, 1, 5);
WeldJoint weld = new WeldJoint(e1.body, e2.body);
space.Add(weld);
// PointOnLine Joint
// create the line
e1 = createBox(new Vector3(60, 5, -20), 1, 1, 5);
e1.body.BecomeKinematic();
e2 = createBox(new Vector3(60, 10, -10), 1, 1, 1);
PointOnLineJoint pol = new PointOnLineJoint(e1.body, e2.body, new Vector3(60, 5, -20), new Vector3(0, 0, -1), new Vector3(60, 5, -10));
space.Add(pol);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
/// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
/// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
/// </summary>
public SwivelHingeJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
AngularJoint = new SwivelHingeAngularJoint();
HingeLimit = new RevoluteLimit();
HingeMotor = new RevoluteMotor();
TwistLimit = new TwistLimit();
TwistMotor = new TwistMotor();
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate in world space.</param>
public UniversalJoint(Entity connectionA, Entity connectionB, Vector3 anchor)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
TwistJoint = new TwistJoint(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
Limit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
Motor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
Limit.IsActive = false;
Motor.IsActive = false;
Add(BallSocketJoint);
Add(TwistJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MoreConstraintsTestDemo(DemosGame game)
: base(game)
{
Box boxA = new Box(new Vector3(0, 5, 0), 1, 2, 1, 10);
Box boxB = new Box(new Vector3(0, 8, 0), 1, 2, 1, 10);
boxA.Orientation = Quaternion.CreateFromYawPitchRoll(0, MathHelper.PiOver4, 0);
boxB.Orientation = Quaternion.CreateFromYawPitchRoll(MathHelper.PiOver4, 0, 0);
WeldJoint weld = new WeldJoint(boxA, boxB);
Space.Add(boxA);
Space.Add(boxB);
Space.Add(weld);
boxA = new Box(new Vector3(3, 5, 0), 1, 2, 1, 10);
boxB = new Box(new Vector3(3, 8, 0), 1, 2, 1, 10);
boxA.Orientation = Quaternion.CreateFromYawPitchRoll(0, MathHelper.PiOver4, 0);
boxB.Orientation = Quaternion.CreateFromYawPitchRoll(MathHelper.PiOver4, 0, 0);
BallSocketJoint ballSocket = new BallSocketJoint(boxA, boxB, (boxA.Position + boxB.Position) / 2);
AngularMotor angularMotor = new AngularMotor(boxA, boxB);
angularMotor.Settings.Mode = MotorMode.Servomechanism;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocket);
Space.Add(angularMotor);
Box ground = new Box(new Vector3(0, 0, 0), 10, 1, 10);
Space.Add(ground);
game.Camera.Position = new Vector3(0, 6, 15);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param>
public SwivelHingeJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 hingeAxis)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB);
HingeLimit = new RevoluteLimit(connectionA, connectionB);
HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
TwistLimit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
TwistMotor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
HingeLimit.IsActive = false;
HingeMotor.IsActive = false;
TwistLimit.IsActive = false;
TwistMotor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
System.Numerics.Vector3 baseAxis = anchor - connectionA.position;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
{
baseAxis = connectionB.position - anchor;
}
baseAxis -= Vector3Ex.Dot(baseAxis, hingeAxis) * hingeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Right);
}
}
HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3Ex.Dot(baseAxis, hingeAxis) * hingeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Right);
}
}
HingeLimit.TestAxis = baseAxis;
HingeMotor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
private void CreateWorld()
{
//Robot arm scene
Owner.Space = new Space();
Owner.Space.ForceUpdater.Gravity = new Vector3(0, -Owner.GRAVITY, 0);
Entity ground = new Box(new Vector3(0, -0.5f, 0), 30, 1, 30);
Owner.Space.Add(ground);
var armBase = new Box(new Vector3(0, 0.25f, 0), 1, 0.5f, 1);
Owner.Space.Add(armBase);
var lowerArm = new Box(armBase.Position + new Vector3(0, armBase.Height / 2 + 0.75f, 0), 0.5f, 1.5f, 0.5f, 1f);
Owner.Space.Add(lowerArm);
var upperArm = new Box(lowerArm.Position + new Vector3(0, lowerArm.Height / 2 + 1, 0), 0.25f, 2, 0.25f, 0.1f);
Owner.Space.Add(upperArm);
pole = new Cylinder(upperArm.Position + new Vector3(0, upperArm.Height / 2 + 2, 0), 4, 0.0625f, 0.01f);
Owner.Space.Add(pole);
sphere = new Sphere(pole.Position + new Vector3(0, pole.Height / 2, 0), 0.25f, 0.001f);
Owner.Space.Add(sphere);
//Lower arm to base joint
shoulder = new RevoluteJoint(armBase, lowerArm, armBase.Position + new Vector3(0, armBase.Height / 2, 0), Vector3.Forward);
shoulder.Motor.IsActive = true;
shoulder.Motor.Settings.Mode = Owner.MOTOR_MODE;
shoulder.Motor.Settings.MaximumForce = 25;
shoulder.Limit.IsActive = true;
float[] limits = GetLimits();
shoulder.Limit.MinimumAngle = -MathHelper.Pi * limits[2];
shoulder.Limit.MaximumAngle = MathHelper.Pi * limits[2];
shoulder.Limit.Bounciness = 0.0f;
Owner.Space.Add(shoulder);
//Upper arm to lower arm joint
elbow = new RevoluteJoint(lowerArm, upperArm, lowerArm.Position + new Vector3(0, lowerArm.Height / 2, 0), Vector3.Left);
elbow.Motor.IsActive = true;
if (Owner.ELBOW_FIXED)
{
elbow.Motor.Settings.Mode = MotorMode.Servomechanism;
}
else
{
elbow.Motor.Settings.Mode = Owner.MOTOR_MODE;
}
elbow.Motor.Settings.MaximumForce = 2500;
elbow.Limit.IsActive = true;
elbow.Limit.MinimumAngle = -MathHelper.Pi * limits[1];
elbow.Limit.MaximumAngle = MathHelper.Pi * limits[1];
elbow.Limit.Bounciness = 0.0f;
Owner.Space.Add(elbow);
if (Owner.POLE_DOF == 1)
{
//Upper arm to pole joint
wrist = new RevoluteJoint(upperArm, pole, upperArm.Position + new Vector3(0, upperArm.Height / 2, 0), Vector3.Forward);
wrist.Motor.IsActive = false;
wrist.Motor.Settings.Mode = Owner.MOTOR_MODE;
wrist.Motor.Settings.MaximumForce = 2500;
wrist.Limit.IsActive = true;
wrist.Limit.MinimumAngle = -MathHelper.Pi * limits[0];
wrist.Limit.MaximumAngle = -MathHelper.Pi * limits[0];
wrist.Limit.Bounciness = 0.0f;
Owner.Space.Add(wrist);
}
else
{
socket = new BallSocketJoint(upperArm, pole, upperArm.Position + new Vector3(0, upperArm.Height / 2, 0));
Owner.Space.Add(socket);
}
//Fixed pole to sphere joint
var norotation = new RevoluteJoint(pole, sphere, pole.Position + new Vector3(0, pole.Height / 2 - 0.5f, 0), Vector3.Forward);
norotation.Limit.IsActive = true;
norotation.Limit.MinimumAngle = 0.0f;
norotation.Limit.MaximumAngle = 0.0f;
Owner.Space.Add(norotation);
CollisionRules.AddRule(armBase, lowerArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(lowerArm, upperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(upperArm, pole, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(pole, sphere, CollisionRule.NoBroadPhase);
}
public Ragdoll()
{
#region Ragdoll Entities
//Create the ragdoll's bones.
var pelvis = new Box(Vector3.Zero, .5m, .28m, .33m, 20);
var torsoBottom = new Box(pelvis.Position + new Vector3(0, .3m, 0), .42m, .48m, .3m, 15);
var torsoTop = new Box(torsoBottom.Position + new Vector3(0, .3m, 0), .5m, .38m, .32m, 20);
var neck = new Box(torsoTop.Position + new Vector3(0, .2m, .04m), .19m, .24m, .2m, 5);
var head = new Sphere(neck.Position + new Vector3(0, .22m, -.04m), .19m, 7);
var leftUpperArm = new Box(torsoTop.Position + new Vector3(-.46m, .1m, 0), .52m, .19m, .19m, 6);
var leftForearm = new Box(leftUpperArm.Position + new Vector3(-.5m, 0, 0), .52m, .18m, .18m, 5);
var leftHand = new Box(leftForearm.Position + new Vector3(-.35m, 0, 0), .28m, .13m, .22m, 4);
var rightUpperArm = new Box(torsoTop.Position + new Vector3(.46m, .1m, 0), .52m, .19m, .19m, 6);
var rightForearm = new Box(rightUpperArm.Position + new Vector3(.5m, 0, 0), .52m, .18m, .18m, 5);
var rightHand = new Box(rightForearm.Position + new Vector3(.35m, 0, 0), .28m, .13m, .22m, 4);
var leftThigh = new Box(pelvis.Position + new Vector3(-.15m, -.4m, 0), .23m, .63m, .23m, 10);
var leftShin = new Box(leftThigh.Position + new Vector3(0, -.6m, 0), .21m, .63m, .21m, 7);
var leftFoot = new Box(leftShin.Position + new Vector3(0, -.35m, -.1m), .23m, .15m, .43m, 5);
var rightThigh = new Box(pelvis.Position + new Vector3(.15m, -.4m, 0), .23m, .63m, .23m, 10);
var rightShin = new Box(rightThigh.Position + new Vector3(0, -.6m, 0), .21m, .63m, .21m, 7);
var rightFoot = new Box(rightShin.Position + new Vector3(0, -.35m, -.1m), .23m, .15m, .43m, 5);
#endregion
#region Bone List
//Make a convenient list of all of the bones.
bones.Add(pelvis);
bones.Add(torsoBottom);
bones.Add(torsoTop);
bones.Add(neck);
bones.Add(head);
bones.Add(leftUpperArm);
bones.Add(leftForearm);
bones.Add(leftHand);
bones.Add(rightUpperArm);
bones.Add(rightForearm);
bones.Add(rightHand);
bones.Add(leftThigh);
bones.Add(leftShin);
bones.Add(leftFoot);
bones.Add(rightThigh);
bones.Add(rightShin);
bones.Add(rightFoot);
#endregion
#region Collision Rules
//Prevent adjacent limbs from colliding.
CollisionRules.AddRule(pelvis, torsoBottom, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoBottom, torsoTop, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoTop, neck, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(neck, head, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(head, torsoTop, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoTop, leftUpperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftUpperArm, leftForearm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftForearm, leftHand, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoTop, rightUpperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightUpperArm, rightForearm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightForearm, rightHand, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(pelvis, leftThigh, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftThigh, leftShin, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftThigh, torsoBottom, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftShin, leftFoot, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(pelvis, rightThigh, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightThigh, rightShin, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightThigh, torsoBottom, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightShin, rightFoot, CollisionRule.NoBroadPhase);
#endregion
//Create the constraints between the bones.
#region Pelvis up to Head Constraints
var pelvisToTorsoBottomBallSocketJoint = new BallSocketJoint(pelvis, torsoBottom, pelvis.Position + new Vector3(0, .1m, 0));
var pelvisToTorsoBottomTwistLimit = new TwistLimit(pelvis, torsoBottom, Vector3.Up, Vector3.Up, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var pelvisToTorsoBottomSwingLimit = new SwingLimit(pelvis, torsoBottom, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var pelvisToTorsoBottomMotor = new AngularMotor(pelvis, torsoBottom);
pelvisToTorsoBottomMotor.Settings.VelocityMotor.Softness = .05m;
var torsoBottomToTorsoTopBallSocketJoint = new BallSocketJoint(torsoBottom, torsoTop, torsoBottom.Position + new Vector3(0, .25m, 0));
var torsoBottomToTorsoTopSwingLimit = new SwingLimit(torsoBottom, torsoTop, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var torsoBottomToTorsoTopTwistLimit = new TwistLimit(torsoBottom, torsoTop, Vector3.Up, Vector3.Up, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var torsoBottomToTorsoTopMotor = new AngularMotor(torsoBottom, torsoTop);
torsoBottomToTorsoTopMotor.Settings.VelocityMotor.Softness = .05m;
var torsoTopToNeckBallSocketJoint = new BallSocketJoint(torsoTop, neck, torsoTop.Position + new Vector3(0, .15m, .05m));
var torsoTopToNeckSwingLimit = new SwingLimit(torsoTop, neck, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var torsoTopToNeckTwistLimit = new TwistLimit(torsoTop, neck, Vector3.Up, Vector3.Up, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var torsoTopToNeckMotor = new AngularMotor(torsoTop, neck);
torsoTopToNeckMotor.Settings.VelocityMotor.Softness = .1m;
var neckToHeadBallSocketJoint = new BallSocketJoint(neck, head, neck.Position + new Vector3(0, .1m, .05m));
var neckToHeadTwistLimit = new TwistLimit(neck, head, Vector3.Up, Vector3.Up, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var neckToHeadSwingLimit = new SwingLimit(neck, head, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var neckToHeadMotor = new AngularMotor(neck, head);
neckToHeadMotor.Settings.VelocityMotor.Softness = .1m;
#endregion
#region Left Arm
var torsoTopToLeftArmBallSocketJoint = new BallSocketJoint(torsoTop, leftUpperArm, torsoTop.Position + new Vector3(-.3m, .1m, 0));
var torsoTopToLeftArmEllipseLimit = new EllipseSwingLimit(torsoTop, leftUpperArm, Vector3.Left, MathHelper.Pi * .75m, MathHelper.PiOver2);
var torsoTopToLeftArmTwistLimit = new TwistLimit(torsoTop, leftUpperArm, Vector3.Left, Vector3.Left, -MathHelper.PiOver2, MathHelper.PiOver2);
var torsoTopToLeftArmMotor = new AngularMotor(torsoTop, leftUpperArm);
torsoTopToLeftArmMotor.Settings.VelocityMotor.Softness = .2m;
var leftUpperArmToLeftForearmSwivelHingeJoint = new SwivelHingeJoint(leftUpperArm, leftForearm, leftUpperArm.Position + new Vector3(-.28m, 0, 0), Vector3.Up);
leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.IsActive = true;
leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.IsActive = true;
leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.MinimumAngle = -MathHelper.Pi / 8;
leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.MaximumAngle = MathHelper.Pi / 8;
leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.MinimumAngle = -MathHelper.Pi * .8m;
leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.MaximumAngle = 0;
//The SwivelHingeJoint has motors, but they are separately defined for twist/bending.
//The AngularMotor covers all degrees of freedom.
var leftUpperArmToLeftForearmMotor = new AngularMotor(leftUpperArm, leftForearm);
leftUpperArmToLeftForearmMotor.Settings.VelocityMotor.Softness = .3m;
var leftForearmToLeftHandBallSocketJoint = new BallSocketJoint(leftForearm, leftHand, leftForearm.Position + new Vector3(-.2m, 0, 0));
var leftForearmToLeftHandEllipseSwingLimit = new EllipseSwingLimit(leftForearm, leftHand, Vector3.Left, MathHelper.PiOver2, MathHelper.Pi / 6);
var leftForearmToLeftHandTwistLimit = new TwistLimit(leftForearm, leftHand, Vector3.Left, Vector3.Left, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var leftForearmToLeftHandMotor = new AngularMotor(leftForearm, leftHand);
leftForearmToLeftHandMotor.Settings.VelocityMotor.Softness = .4m;
#endregion
#region Right Arm
var torsoTopToRightArmBallSocketJoint = new BallSocketJoint(torsoTop, rightUpperArm, torsoTop.Position + new Vector3(.3m, .1m, 0));
var torsoTopToRightArmEllipseLimit = new EllipseSwingLimit(torsoTop, rightUpperArm, Vector3.Right, MathHelper.Pi * .75m, MathHelper.PiOver2);
var torsoTopToRightArmTwistLimit = new TwistLimit(torsoTop, rightUpperArm, Vector3.Right, Vector3.Right, -MathHelper.PiOver2, MathHelper.PiOver2);
var torsoTopToRightArmMotor = new AngularMotor(torsoTop, rightUpperArm);
torsoTopToRightArmMotor.Settings.VelocityMotor.Softness = .2m;
var rightUpperArmToRightForearmSwivelHingeJoint = new SwivelHingeJoint(rightUpperArm, rightForearm, rightUpperArm.Position + new Vector3(.28m, 0, 0), Vector3.Up);
rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.IsActive = true;
rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.IsActive = true;
rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.MinimumAngle = -MathHelper.Pi / 8;
rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.MaximumAngle = MathHelper.Pi / 8;
rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.MinimumAngle = 0;
rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.MaximumAngle = MathHelper.Pi * .8m;
//The SwivelHingeJoint has motors, but they are separately defined for twist/bending.
//The AngularMotor covers all degrees of freedom.
var rightUpperArmToRightForearmMotor = new AngularMotor(rightUpperArm, rightForearm);
rightUpperArmToRightForearmMotor.Settings.VelocityMotor.Softness = .3m;
var rightForearmToRightHandBallSocketJoint = new BallSocketJoint(rightForearm, rightHand, rightForearm.Position + new Vector3(.2m, 0, 0));
var rightForearmToRightHandEllipseSwingLimit = new EllipseSwingLimit(rightForearm, rightHand, Vector3.Right, MathHelper.PiOver2, MathHelper.Pi / 6);
var rightForearmToRightHandTwistLimit = new TwistLimit(rightForearm, rightHand, Vector3.Right, Vector3.Right, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var rightForearmToRightHandMotor = new AngularMotor(rightForearm, rightHand);
rightForearmToRightHandMotor.Settings.VelocityMotor.Softness = .4m;
#endregion
#region Left Leg
var pelvisToLeftThighBallSocketJoint = new BallSocketJoint(pelvis, leftThigh, pelvis.Position + new Vector3(-.15m, -.1m, 0));
var pelvisToLeftThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, leftThigh, Vector3.Normalize(new Vector3(-.2m, -1, -.6m)), MathHelper.Pi * .7m, MathHelper.PiOver4);
pelvisToLeftThighEllipseSwingLimit.LocalTwistAxisB = Vector3.Down;
var pelvisToLeftThighTwistLimit = new TwistLimit(pelvis, leftThigh, Vector3.Down, Vector3.Down, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var pelvisToLeftThighMotor = new AngularMotor(pelvis, leftThigh);
pelvisToLeftThighMotor.Settings.VelocityMotor.Softness = .1m;
var leftThighToLeftShinRevoluteJoint = new RevoluteJoint(leftThigh, leftShin, leftThigh.Position + new Vector3(0, -.3m, 0), Vector3.Right);
leftThighToLeftShinRevoluteJoint.Limit.IsActive = true;
leftThighToLeftShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8m;
leftThighToLeftShinRevoluteJoint.Limit.MaximumAngle = 0;
leftThighToLeftShinRevoluteJoint.Motor.IsActive = true;
leftThighToLeftShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2m;
var leftShinToLeftFootBallSocketJoint = new BallSocketJoint(leftShin, leftFoot, leftShin.Position + new Vector3(0, -.3m, 0));
var leftShinToLeftFootSwingLimit = new SwingLimit(leftShin, leftFoot, Vector3.Forward, Vector3.Forward, MathHelper.Pi / 8);
var leftShinToLeftFootTwistLimit = new TwistLimit(leftShin, leftFoot, Vector3.Down, Vector3.Forward, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var leftShinToLeftFootMotor = new AngularMotor(leftShin, leftFoot);
leftShinToLeftFootMotor.Settings.VelocityMotor.Softness = .2m;
#endregion
#region Right Leg
var pelvisToRightThighBallSocketJoint = new BallSocketJoint(pelvis, rightThigh, pelvis.Position + new Vector3(.15m, -.1m, 0));
var pelvisToRightThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, rightThigh, Vector3.Normalize(new Vector3(.2m, -1, -.6m)), MathHelper.Pi * .7m, MathHelper.PiOver4);
pelvisToRightThighEllipseSwingLimit.LocalTwistAxisB = Vector3.Down;
var pelvisToRightThighTwistLimit = new TwistLimit(pelvis, rightThigh, Vector3.Down, Vector3.Down, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var pelvisToRightThighMotor = new AngularMotor(pelvis, rightThigh);
pelvisToRightThighMotor.Settings.VelocityMotor.Softness = .1m;
var rightThighToRightShinRevoluteJoint = new RevoluteJoint(rightThigh, rightShin, rightThigh.Position + new Vector3(0, -.3m, 0), Vector3.Right);
rightThighToRightShinRevoluteJoint.Limit.IsActive = true;
rightThighToRightShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8m;
rightThighToRightShinRevoluteJoint.Limit.MaximumAngle = 0;
rightThighToRightShinRevoluteJoint.Motor.IsActive = true;
rightThighToRightShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2m;
var rightShinToRightFootBallSocketJoint = new BallSocketJoint(rightShin, rightFoot, rightShin.Position + new Vector3(0, -.3m, 0));
var rightShinToRightFootSwingLimit = new SwingLimit(rightShin, rightFoot, Vector3.Forward, Vector3.Forward, MathHelper.Pi / 8);
var rightShinToRightFootTwistLimit = new TwistLimit(rightShin, rightFoot, Vector3.Down, Vector3.Forward, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var rightShinToRightFootMotor = new AngularMotor(rightShin, rightFoot);
rightShinToRightFootMotor.Settings.VelocityMotor.Softness = .2m;
#endregion
#region Joint List
//Collect the joints.
joints.Add(pelvisToTorsoBottomBallSocketJoint);
joints.Add(pelvisToTorsoBottomTwistLimit);
joints.Add(pelvisToTorsoBottomSwingLimit);
joints.Add(pelvisToTorsoBottomMotor);
joints.Add(torsoBottomToTorsoTopBallSocketJoint);
joints.Add(torsoBottomToTorsoTopTwistLimit);
joints.Add(torsoBottomToTorsoTopSwingLimit);
joints.Add(torsoBottomToTorsoTopMotor);
joints.Add(torsoTopToNeckBallSocketJoint);
joints.Add(torsoTopToNeckTwistLimit);
joints.Add(torsoTopToNeckSwingLimit);
joints.Add(torsoTopToNeckMotor);
joints.Add(neckToHeadBallSocketJoint);
joints.Add(neckToHeadTwistLimit);
joints.Add(neckToHeadSwingLimit);
joints.Add(neckToHeadMotor);
joints.Add(torsoTopToLeftArmBallSocketJoint);
joints.Add(torsoTopToLeftArmEllipseLimit);
joints.Add(torsoTopToLeftArmTwistLimit);
joints.Add(torsoTopToLeftArmMotor);
joints.Add(leftUpperArmToLeftForearmSwivelHingeJoint);
joints.Add(leftUpperArmToLeftForearmMotor);
joints.Add(leftForearmToLeftHandBallSocketJoint);
joints.Add(leftForearmToLeftHandEllipseSwingLimit);
joints.Add(leftForearmToLeftHandTwistLimit);
joints.Add(leftForearmToLeftHandMotor);
joints.Add(torsoTopToRightArmBallSocketJoint);
joints.Add(torsoTopToRightArmEllipseLimit);
joints.Add(torsoTopToRightArmTwistLimit);
joints.Add(torsoTopToRightArmMotor);
joints.Add(rightUpperArmToRightForearmSwivelHingeJoint);
joints.Add(rightUpperArmToRightForearmMotor);
joints.Add(rightForearmToRightHandBallSocketJoint);
joints.Add(rightForearmToRightHandEllipseSwingLimit);
joints.Add(rightForearmToRightHandTwistLimit);
joints.Add(rightForearmToRightHandMotor);
joints.Add(pelvisToLeftThighBallSocketJoint);
joints.Add(pelvisToLeftThighEllipseSwingLimit);
joints.Add(pelvisToLeftThighTwistLimit);
joints.Add(pelvisToLeftThighMotor);
joints.Add(leftThighToLeftShinRevoluteJoint);
joints.Add(leftShinToLeftFootBallSocketJoint);
joints.Add(leftShinToLeftFootSwingLimit);
joints.Add(leftShinToLeftFootTwistLimit);
joints.Add(leftShinToLeftFootMotor);
joints.Add(pelvisToRightThighBallSocketJoint);
joints.Add(pelvisToRightThighEllipseSwingLimit);
joints.Add(pelvisToRightThighTwistLimit);
joints.Add(pelvisToRightThighMotor);
joints.Add(rightThighToRightShinRevoluteJoint);
joints.Add(rightShinToRightFootBallSocketJoint);
joints.Add(rightShinToRightFootSwingLimit);
joints.Add(rightShinToRightFootTwistLimit);
joints.Add(rightShinToRightFootMotor);
#endregion
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public ReverseTrikeDemo(DemosGame game)
: base(game)
{
game.Camera.Position = new Microsoft.Xna.Framework.Vector3(0, 2, 15);
game.Camera.Yaw = 0;
game.Camera.Pitch = 0;
Space.Add(new Box(new Vector3(0, -5, 0), 20, 1, 20));
var body = new Box(new Vector3(0, 0, 0), 2, 1, 3, 10);
body.CollisionInformation.LocalPosition = new Vector3(0, .8f, 0);
Space.Add(body);
#region First Wheel
var wheel = new Cylinder(body.Position + new Vector3(-1.3f, 0, -1.5f), .2f, .5f, 4);
wheel.Material = new Material(1.5f, 1.5f, 0);
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
var ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position);
var swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
//Motorize the wheel.
drivingMotor1 = new RevoluteMotor(body, wheel, Vector3.Left);
drivingMotor1.Settings.VelocityMotor.Softness = .2f;
//Let it roll when the user isn't giving specific commands.
drivingMotor1.IsActive = false;
steeringMotor1 = new RevoluteMotor(body, wheel, Vector3.Up);
steeringMotor1.Settings.Mode = MotorMode.Servomechanism;
//The constructor makes a guess about how to set up the constraint.
//It can't always be right since it doesn't have all the information;
//in this case, it chooses the basis and test axis incorrectly.
//This leads to a 'flipping' behavior when the wheel is rolling
//(the test axis is 'rolling' with the wheel, and passes over
//a singularity which causes a flip).
//To fix this, we configure the constraint directly.
//The basis is aligned with how the wheel is set up; we choose 'up' as
//the motorized axis, and right/forward to define the angle measurement plane.
//The test axis is set to be perpendicular to the wheel's rotation so that
//it only measures the steering angle.
//If you're curious, the angle measurement is just a Math.Atan2.
//The current world test axis is dotted against the two plane axes (Right and Forward here).
//This gives an x and y value. These can be plugged into Atan2 just like when
//you compute an angle on a normal 2d graph.
steeringMotor1.Basis.SetWorldAxes(Vector3.Up, Vector3.Right, Vector3.Forward);
steeringMotor1.TestAxis = Vector3.Right;
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(ballSocketJoint);
Space.Add(swivelHingeAngularJoint);
Space.Add(drivingMotor1);
Space.Add(steeringMotor1);
#endregion
#region Second Wheel
wheel = new Cylinder(body.Position + new Vector3(1.3f, 0, -1.5f), .2f, .5f, 4);
wheel.Material = new Material(1.5f, 1.5f, 0);
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position);
swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
//Motorize the wheel.
drivingMotor2 = new RevoluteMotor(body, wheel, Vector3.Left);
drivingMotor2.Settings.VelocityMotor.Softness = .2f;
//Let it roll when the user isn't giving specific commands.
drivingMotor2.IsActive = false;
steeringMotor2 = new RevoluteMotor(body, wheel, Vector3.Up);
steeringMotor2.Settings.Mode = MotorMode.Servomechanism;
//Configure the motor. See wheel 1 for more description.
steeringMotor2.Basis.SetWorldAxes(Vector3.Up, Vector3.Right, Vector3.Forward);
steeringMotor2.TestAxis = Vector3.Right;
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(ballSocketJoint);
Space.Add(swivelHingeAngularJoint);
Space.Add(drivingMotor2);
Space.Add(steeringMotor2);
#endregion
#region Third Wheel
wheel = new Cylinder(body.Position + new Vector3(0, -.3f, 1.5f), .2f, .5f, 4);
wheel.Material = new Material(1.5f, 1.5f, 0);
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position);
//Notice that the third wheel isn't a swivel hinge, it's just a revolute axis.
//This lets it roll, but prevents flopping around like the wheels of a grocery cart.
//Could have used a RevoluteJoint solver group here, but this shows it's possible to do
//the same things without using the combo-constraints.
var revoluteAngularJoint = new RevoluteAngularJoint(body, wheel, Vector3.Right);
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(ballSocketJoint);
Space.Add(revoluteAngularJoint);
#endregion
int xLength = 256;
int zLength = 256;
float xSpacing = 8f;
float zSpacing = 8f;
var heights = new float[xLength, zLength];
for (int i = 0; i < xLength; i++)
{
for (int j = 0; j < zLength; j++)
{
float x = i - xLength / 2;
float z = j - zLength / 2;
//heights[i,j] = (float)(x * y / 1000f);
heights[i, j] = (float)(10 * (Math.Sin(x / 8) + Math.Sin(z / 8)));
//heights[i,j] = 3 * (float)Math.Sin(x * y / 100f);
//heights[i,j] = (x * x * x * y - y * y * y * x) / 1000f;
}
}
//Create the terrain.
var terrain = new Terrain(heights, new AffineTransform(
new Vector3(xSpacing, 1, zSpacing),
Quaternion.Identity,
new Vector3(-xLength * xSpacing / 2, -10, -zLength * zSpacing / 2)));
Space.Add(terrain);
game.ModelDrawer.Add(terrain);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public SelfCollidingClothDemo(DemosGame game)
: base(game)
{
//Joints can also act like springs by modifying their springSettings.
//Though using a bunch of DistanceJoint objects can be slower than just applying direct spring forces,
//it is significantly more stable and allows rigid structures.
//The extra stability can make it useful for cloth-like simulations.
Entity latticePiece;
BallSocketJoint joint;
NarrowPhaseHelper.Factories.BoxBox.Count = 10000;
NarrowPhaseHelper.Factories.BoxSphere.Count = 5000;
int numColumns = 70;
int numRows = 70;
float xSpacing = .5f;
float zSpacing = .5f;
var lattice = new Entity[numRows, numColumns];
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
latticePiece = new Box(
new Vector3(
xSpacing * i - (numRows - 1) * xSpacing / 2f,
15.58f,
2 + zSpacing * j - (numColumns - 1) * zSpacing / 2f),
xSpacing, .1f, zSpacing, 1);
//latticePiece.LocalInertiaTensorInverse = new Matrix3X3();
//latticePiece.Tag = "noDisplayObject"; //The joint lines are visible enough; don't add a sphere model for this sphere.
lattice[i, j] = latticePiece;
latticePiece.Material.KineticFriction = 0;
Space.Add(latticePiece);
}
}
//The joints composing the cloth can have their max iterations set independently from the solver iterations.
//More iterations (up to the solver's own max) will increase the quality at the cost of speed.
int clothIterations = 10;
//So while the above prevents joints from using more than 1 iteration, setting the solver's iteration limit can lower the
//rest of the solving load (collisions).
Space.Solver.IterationLimit = 10;
float damping = 5000, stiffness = 5000;
float starchDamping = 5000, starchStiffness = 500;
//Loop through the grid and set up the joints.
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
if (i == 0 && j + 1 < numColumns)
{
//Add in column connections for left edge.
joint = new BallSocketJoint(lattice[0, j], lattice[0, j + 1], lattice[0, j].Position + new Vector3(-xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
}
if (i == numRows - 1 && j + 1 < numColumns)
{
//Add in column connections for right edge.
joint = new BallSocketJoint(lattice[numRows - 1, j], lattice[numRows - 1, j + 1], lattice[numRows - 1, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
}
if (i + 1 < numRows && j == 0)
{
//Add in row connections for top edge.
joint = new BallSocketJoint(lattice[i, 0], lattice[i + 1, 0], lattice[i, 0].Position + new Vector3(xSpacing / 2, 0, -zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
}
if (i + 1 < numRows && j == numColumns - 1)
{
//Add in row connections for bottom edge.
joint = new BallSocketJoint(lattice[i, numColumns - 1], lattice[i + 1, numColumns - 1], lattice[i, numColumns - 1].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
}
if (i + 1 < numRows && j + 1 < numColumns)
{
//Add in interior connections.
joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.DampingConstant = damping; joint.SpringSettings.StiffnessConstant = stiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
}
if (i + 2 < numRows && j + 2 < numColumns)
{
//Add in skipping 'starch' connections.
joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
joint.SpringSettings.DampingConstant = starchDamping; joint.SpringSettings.StiffnessConstant = starchStiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
joint.SpringSettings.DampingConstant = starchDamping; joint.SpringSettings.StiffnessConstant = starchStiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
joint.SpringSettings.DampingConstant = starchDamping; joint.SpringSettings.StiffnessConstant = starchStiffness;
joint.SolverSettings.MaximumIterations = clothIterations;
Space.Add(joint);
}
//Add in collision rules.
if (j - 1 >= 0)
{
if (i - 1 >= 0)
{
CollisionRules.AddRule(lattice[i, j], lattice[i - 1, j - 1], CollisionRule.NoBroadPhase);
}
CollisionRules.AddRule(lattice[i, j], lattice[i, j - 1], CollisionRule.NoBroadPhase);
if (i + 1 < numRows)
{
CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j - 1], CollisionRule.NoBroadPhase);
}
}
if (i + 1 < numRows)
{
CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j], CollisionRule.NoBroadPhase);
}
}
}
//Add some ground.
Space.Add(new Sphere(new Vector3(7, 0, 0), 10));
Space.Add(new Box(new Vector3(0, -20.5f, 0), 100f, 10, 100f));
game.Camera.Position = new Microsoft.Xna.Framework.Vector3(0, 5, 25);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public UnfortunateGuyDemo(DemosGame game)
: base(game)
{
Entity ground = new Box(Vector3.Zero, 10, 1, 10);
Space.Add(ground);
//Rather than add basically redundant code for every limb, this demo
//focuses on a single arm, showing some extra details and limits.
//Make the torso.
var bodies = new List <CompoundShapeEntry>()
{
new CompoundShapeEntry(new BoxShape(2, 1.5m, 1), new Vector3(-1, 3, 0), 50),
new CompoundShapeEntry(new SphereShape(.45m), new Vector3(.4m, 3, 0), 1),
new CompoundShapeEntry(new SphereShape(.25m), new Vector3(-1.9m, 3.5m, 0), 1),
new CompoundShapeEntry(new SphereShape(.25m), new Vector3(-1.9m, 2.5m, 0), 1),
new CompoundShapeEntry(new SphereShape(.25m), new Vector3(-.3m, 2.3m, 0), 1)
};
var torso = new CompoundBody(bodies, 54);
Space.Add(torso);
//Make the upper arm.
Entity upperArm = new Box(torso.Position + new Vector3(1, 1.4m, 0), .4m, 1.2m, .4m, 8);
Space.Add(upperArm);
//A ball socket joint will handle the linear degrees of freedom between the two entities.
var ballSocketJoint = new BallSocketJoint(torso, upperArm, torso.Position + new Vector3(1, .7m, 0));
Space.Add(ballSocketJoint);
//Shoulders don't have a simple limit. The EllipseSwingLimit allows angles within an ellipse, which is closer to how some joints function
//than just flat planes (like two RevoluteLimits) or a single angle (like SwingLimits).
var swingLimit = new EllipseSwingLimit(torso, upperArm, Vector3.Up, MathHelper.PiOver2, MathHelper.PiOver4 * 3);
Space.Add(swingLimit);
//Upper arms can't spin around forever.
var twistLimit = new TwistLimit(torso, upperArm, Vector3.Up, Vector3.Up, -MathHelper.PiOver4 / 2, MathHelper.PiOver4);
twistLimit.SpringSettings.Stiffness = 100;
twistLimit.SpringSettings.Damping = 100;
Space.Add(twistLimit);
//Make the lower arm.
Entity lowerArm = new Box(upperArm.Position + new Vector3(0, 1.4m, 0), .35m, 1.3m, .35m, 8);
Space.Add(lowerArm);
var elbow = new SwivelHingeJoint(upperArm, lowerArm, upperArm.Position + new Vector3(0, .6m, 0), Vector3.Forward);
//Forearm can twist a little.
elbow.TwistLimit.IsActive = true;
elbow.TwistLimit.MinimumAngle = -MathHelper.PiOver4 / 2;
elbow.TwistLimit.MaximumAngle = MathHelper.PiOver4 / 2;
elbow.TwistLimit.SpringSettings.Damping = 100;
elbow.TwistLimit.SpringSettings.Stiffness = 100;
//The elbow is like a hinge, but it can't hyperflex.
elbow.HingeLimit.IsActive = true;
elbow.HingeLimit.MinimumAngle = 0;
elbow.HingeLimit.MaximumAngle = MathHelper.Pi * .7m;
Space.Add(elbow);
Entity hand = new Box(lowerArm.Position + new Vector3(0, .9m, 0), .4m, .55m, .25m, 3);
Space.Add(hand);
ballSocketJoint = new BallSocketJoint(lowerArm, hand, lowerArm.Position + new Vector3(0, .7m, 0));
Space.Add(ballSocketJoint);
//Wrists can use an ellipse limit too.
swingLimit = new EllipseSwingLimit(lowerArm, hand, Vector3.Up, MathHelper.PiOver4, MathHelper.PiOver2);
Space.Add(swingLimit);
//Allow a little extra twist beyond the forearm.
twistLimit = new TwistLimit(lowerArm, hand, Vector3.Up, Vector3.Up, -MathHelper.PiOver4 / 2, MathHelper.PiOver4 / 2);
twistLimit.SpringSettings.Stiffness = 100;
twistLimit.SpringSettings.Damping = 100;
Space.Add(twistLimit);
//The hand is pretty floppy without some damping.
var angularMotor = new AngularMotor(lowerArm, hand);
angularMotor.Settings.VelocityMotor.Softness = .5m;
Space.Add(angularMotor);
//Make sure the parts of the arm don't collide.
CollisionRules.AddRule(torso, upperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(lowerArm, upperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(lowerArm, hand, CollisionRule.NoBroadPhase);
game.Camera.Position = new Vector3(0, 4, 20);
}
public JointLimitTestDemo(DemosGame game)
: base(game)
{
float bounciness = 1;
float baseMass = 100;
float armMass = 10;
//DistanceLimit
Box boxA = new Box(new Vector3(-21, 4, 0), 3, 3, 3, baseMass);
Box boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
var distanceLimit = new DistanceLimit(boxA, boxB, boxA.Position, boxB.Position - new Vector3(0, 2, 0), 1, 6);
distanceLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(distanceLimit);
//EllipseSwingLimit
boxA = new Box(new Vector3(-14, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
var ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
var ellipseSwingLimit = new EllipseSwingLimit(boxA, boxB, Vector3.Up, MathHelper.Pi / 1.5f, MathHelper.Pi / 3);
ellipseSwingLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(ellipseSwingLimit);
//LinearAxisLimit
boxA = new Box(new Vector3(-7, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
var pointOnLineJoint = new PointOnLineJoint(boxA, boxB, boxA.Position, Vector3.Up, boxB.Position + new Vector3(0, -2, 0));
var linearAxisLimit = new LinearAxisLimit(boxA, boxB, boxA.Position, boxB.Position + new Vector3(0, -2, 0), Vector3.Up, 0, 4);
linearAxisLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(pointOnLineJoint);
Space.Add(linearAxisLimit);
//RevoluteLimit
boxA = new Box(new Vector3(0, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
var revoluteAngularJoint = new RevoluteAngularJoint(boxA, boxB, Vector3.Forward);
var revoluteLimit = new RevoluteLimit(boxA, boxB, Vector3.Forward, Vector3.Up, -MathHelper.PiOver4, MathHelper.PiOver4);
revoluteLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(revoluteAngularJoint);
Space.Add(revoluteLimit);
//SwingLimit
boxA = new Box(new Vector3(7, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
var swingLimit = new SwingLimit(boxA, boxB, Vector3.Up, Vector3.Up, MathHelper.PiOver4);
swingLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(swingLimit);
//TwistLimit
boxA = new Box(new Vector3(14, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
revoluteAngularJoint = new RevoluteAngularJoint(boxA, boxB, Vector3.Up);
var twistLimit = new TwistLimit(boxA, boxB, Vector3.Up, Vector3.Up, -MathHelper.PiOver4, MathHelper.PiOver4);
twistLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(revoluteAngularJoint);
Space.Add(twistLimit);
Space.Add(new Box(new Vector3(0, 0, 0), 60, 1, 60));
game.Camera.Position = new Vector3(0, 6, 15);
}
protected override void InitializeSpace()
{
//Joints can also act like springs by modifying their springSettings.
//Though using a bunch of DistanceJoint objects can be slower than just applying direct spring forces,
//it is significantly more stable and allows rigid structures.
//The extra stability can make it useful for cloth-like simulations.
Entity latticePiece;
BallSocketJoint joint;
NarrowPhaseHelper.Factories.BoxBox.Count = 4000;
NarrowPhaseHelper.Factories.BoxSphere.Count = 1000;
int numColumns = 40;
int numRows = 40;
Fix64 xSpacing = 1.0m;
Fix64 zSpacing = 1.0m;
var lattice = new Entity[numRows, numColumns];
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
latticePiece = new Box(
new Vector3(
xSpacing * i - (numRows - 1) * xSpacing / 2,
15.58m,
2 + zSpacing * j - (numColumns - 1) * zSpacing / 2),
xSpacing, .2m, zSpacing, 10);
lattice[i, j] = latticePiece;
Space.Add(latticePiece);
}
}
//The joints composing the cloth can have their max iterations set independently from the solver iterations.
//More iterations (up to the solver's own max) will increase the quality at the cost of speed.
int clothIterations = 3;
//So while the above clamps joint iterations, setting the solver's iteration limit can lower the
//rest of the solving load (collisions).
Space.Solver.IterationLimit = 10;
Fix64 damping = 20000, stiffness = 20000;
Fix64 starchDamping = 5000, starchStiffness = 500;
//Loop through the grid and set up the joints.
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numColumns; j++)
{
if (i == 0 && j + 1 < numColumns)
{
//Add in column connections for left edge.
joint = new BallSocketJoint(lattice[0, j], lattice[0, j + 1], lattice[0, j].Position + new Vector3(-xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
}
if (i == numRows - 1 && j + 1 < numColumns)
{
//Add in column connections for right edge.
joint = new BallSocketJoint(lattice[numRows - 1, j], lattice[numRows - 1, j + 1], lattice[numRows - 1, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
}
if (i + 1 < numRows && j == 0)
{
//Add in row connections for top edge.
joint = new BallSocketJoint(lattice[i, 0], lattice[i + 1, 0], lattice[i, 0].Position + new Vector3(xSpacing / 2, 0, -zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
}
if (i + 1 < numRows && j == numColumns - 1)
{
//Add in row connections for bottom edge.
joint = new BallSocketJoint(lattice[i, numColumns - 1], lattice[i + 1, numColumns - 1], lattice[i, numColumns - 1].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
}
if (i + 1 < numRows && j + 1 < numColumns)
{
//Add in interior connections.
joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2));
joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
}
if (i + 2 < numRows && j + 2 < numColumns)
{
//Add in skipping 'starch' connections.
joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing));
joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness;
joint.SolverSettings.MaximumIterationCount = clothIterations;
Space.Add(joint);
}
//Add in collision rules.
if (j - 1 >= 0)
{
if (i - 1 >= 0)
{
CollisionRules.AddRule(lattice[i, j], lattice[i - 1, j - 1], CollisionRule.NoBroadPhase);
}
CollisionRules.AddRule(lattice[i, j], lattice[i, j - 1], CollisionRule.NoBroadPhase);
if (i + 1 < numRows)
{
CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j - 1], CollisionRule.NoBroadPhase);
}
}
if (i + 1 < numRows)
{
CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j], CollisionRule.NoBroadPhase);
}
}
}
//Add some ground.
var sphere = new Sphere(new Vector3(7, 0, 0), 10);
sphere.Material.KineticFriction = .2m;
Space.Add(sphere);
Space.Add(new Box(new Vector3(0, -20.5m, 0), 100, 10, 100));
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public DogbotDemo(DemosGame game)
: base(game)
{
Entity body = new Box(new Vector3(0, 0, 0), 4, 2, 2, 20);
Space.Add(body);
Entity head = new Cone(body.Position + new Vector3(3.2f, .3f, 0), 1.5f, .7f, 4);
head.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
Space.Add(head);
//Attach the head to the body
var universalJoint = new UniversalJoint(body, head, head.Position + new Vector3(-.8f, 0, 0));
Space.Add(universalJoint);
//Keep the head from swinging around too much.
var angleLimit = new SwingLimit(body, head, Vector3.Right, Vector3.Right, MathHelper.PiOver4);
Space.Add(angleLimit);
var tail = new Box(body.Position + new Vector3(-3f, 1f, 0), 1.6f, .1f, .1f, 4);
Space.Add(tail);
//Keep the tail from twisting itself off.
universalJoint = new UniversalJoint(body, tail, tail.Position + new Vector3(.8f, 0, 0));
Space.Add(universalJoint);
//Give 'em some floppy ears.
var ear = new Box(head.Position + new Vector3(-.2f, 0, -.65f), .01f, .7f, .2f, 1);
Space.Add(ear);
var ballSocketJoint = new BallSocketJoint(head, ear, head.Position + new Vector3(-.2f, .35f, -.65f));
Space.Add(ballSocketJoint);
ear = new Box(head.Position + new Vector3(-.2f, 0, .65f), .01f, .7f, .3f, 1);
Space.Add(ear);
ballSocketJoint = new BallSocketJoint(head, ear, head.Position + new Vector3(-.2f, .35f, .65f));
Space.Add(ballSocketJoint);
Box arm;
Cylinder shoulder;
PointOnLineJoint pointOnLineJoint;
//************* First Arm *************//
arm = new Box(body.Position + new Vector3(-1.8f, -.5f, 1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(-1.8f, .3f, 1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
var axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
shoulder.OrientationMatrix *= Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.Pi); //Force the walker's legs out of phase.
//************* Second Arm *************//
arm = new Box(body.Position + new Vector3(1.8f, -.5f, 1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(1.8f, .3f, 1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
//************* Third Arm *************//
arm = new Box(body.Position + new Vector3(-1.8f, -.5f, -1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(-1.8f, .3f, -1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
shoulder.OrientationMatrix *= Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.Pi); //Force the walker's legs out of phase.
//************* Fourth Arm *************//
arm = new Box(body.Position + new Vector3(1.8f, -.5f, -1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(1.8f, .3f, -1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
//Add some ground.
Space.Add(new Box(new Vector3(0, -3.5f, 0), 20f, 1, 20f));
game.Camera.Position = new Vector3(0, 2, 20);
}