/// <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 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 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, ref Vector3 anchor, ref Vector3 hingeAxis)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
Vector3 tmp;
BallSocketJoint.OffsetB.Invert( out tmp );
AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp );
HingeLimit = new RevoluteLimit(connectionA, connectionB);
HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
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.
Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis );
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
connectionB.position.Sub( ref anchor, out baseAxis );
baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis );
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
}
}
HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
connectionB.position.Sub( ref anchor, out baseAxis );
baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis );
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
}
}
HingeLimit.TestAxis = baseAxis;
HingeMotor.TestAxis = baseAxis;
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 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, Vector3 anchor, 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.
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, 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 = Vector3.Cross(hingeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
}
}
HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3.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 = Vector3.Cross(hingeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
}
}
HingeLimit.TestAxis = baseAxis;
HingeMotor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
/// <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, 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);
}
/// <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);
}
/// <summary>
/// Constructs a new constraint which restricts the linear and angular motion between two entities.
/// Uses the average of the two entity positions for the anchor.
/// </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;
Vector3 anchor; GetAnchorGuess( connectionA, connectionB, out anchor );
BallSocketJoint = new BallSocketJoint( connectionA, connectionB, ref anchor );
NoRotationJoint = new NoRotationJoint( connectionA, connectionB );
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.
/// 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);
}
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.
/// </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, System.Numerics.Vector3 anchor, System.Numerics.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.
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, 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 = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3Ex.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 = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.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.
/// </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 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, ref Vector3 anchor)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
Vector3 tmp; BallSocketJoint.OffsetB.Invert( out tmp );
TwistJoint = new TwistJoint(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp);
Limit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
Motor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
Limit.IsActive = false;
Motor.IsActive = false;
Add(BallSocketJoint);
Add(TwistJoint);
Add(Limit);
Add(Motor);
}
void BuildCyclicMesh(Vector3 position)
{
int widthCount = 5;
int heightCount = 5;
var boneMesh = new BoneRelationship[widthCount, heightCount];
for (int i = 0; i < widthCount; i++)
{
for (int j = 0; j < heightCount; j++)
{
var bonePosition = position + new Vector3(i, j, 0);
boneMesh[i, j] = new BoneRelationship(
new Bone(bonePosition, Quaternion.Identity, .4f, .8f),
new Box(bonePosition, .8f, .8f, .8f, 10));
Space.Add(boneMesh[i, j].Entity);
bones.Add(boneMesh[i, j]);
}
}
for (int i = 0; i < widthCount; i++)
{
for (int j = 0; j < heightCount; j++)
{
if (i > 0)
{
var boneA = boneMesh[i, j];
var boneB = boneMesh[i - 1, j];
var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2;
var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor);
CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase);
Space.Add(dynamicsJoint);
joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor));
}
if (j > 0)
{
var boneA = boneMesh[i, j];
var boneB = boneMesh[i, j - 1];
var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2;
var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor);
CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase);
Space.Add(dynamicsJoint);
joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor));
}
if (i > 0 && j > 0)
{
var boneA = boneMesh[i, j];
var boneB = boneMesh[i - 1, j - 1];
var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2;
var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor);
CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase);
Space.Add(dynamicsJoint);
joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor));
}
if (i < widthCount - 1 && j > 0)
{
var boneA = boneMesh[i, j];
var boneB = boneMesh[i + 1, j - 1];
var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2;
var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor);
CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase);
Space.Add(dynamicsJoint);
joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor));
}
}
}
int limbCount = 4;
var previous = boneMesh[boneMesh.GetLength(0) / 2, 0];
for (int i = 0; i < limbCount; i++)
{
Bone bone = new Bone(previous.Bone.Position + new Vector3(0, -0.8f, 0), Quaternion.Identity, .2f, .4f);
Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10);
Space.Add(entity);
var anchor = (entity.Position + previous.Entity.Position) / 2;
Space.Add(new BallSocketJoint(entity, previous.Entity, anchor));
joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor));
CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase);
previous = new BoneRelationship(bone, entity);
bones.Add(previous);
}
previous = boneMesh[boneMesh.GetLength(0) / 2, boneMesh.GetLength(1) - 1];
for (int i = 0; i < limbCount; i++)
{
Bone bone = new Bone(previous.Bone.Position + new Vector3(0, 0.8f, 0), Quaternion.Identity, .2f, .4f);
Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10);
Space.Add(entity);
var anchor = (entity.Position + previous.Entity.Position) / 2;
Space.Add(new BallSocketJoint(entity, previous.Entity, anchor));
CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase);
joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor));
previous = new BoneRelationship(bone, entity);
bones.Add(previous);
}
previous = boneMesh[0, boneMesh.GetLength(1) / 2];
for (int i = 0; i < limbCount; i++)
{
Bone bone = new Bone(previous.Bone.Position + new Vector3(-.8f, 0, 0), Quaternion.Identity, .2f, .4f);
Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10);
Space.Add(entity);
var anchor = (entity.Position + previous.Entity.Position) / 2;
Space.Add(new BallSocketJoint(entity, previous.Entity, anchor));
CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase);
joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor));
previous = new BoneRelationship(bone, entity);
bones.Add(previous);
}
previous = boneMesh[boneMesh.GetLength(0) - 1, boneMesh.GetLength(1) / 2];
for (int i = 0; i < limbCount; i++)
{
Bone bone = new Bone(previous.Bone.Position + new Vector3(0.8f, 0, 0), Quaternion.Identity, .2f, .4f);
Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10);
Space.Add(entity);
var anchor = (entity.Position + previous.Entity.Position) / 2;
Space.Add(new BallSocketJoint(entity, previous.Entity, anchor));
CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase);
joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor));
previous = new BoneRelationship(bone, entity);
bones.Add(previous);
}
}
public Ragdoll()
{
#region Ragdoll Entities
//Create the ragdoll's bones.
var pelvis = new Box(Vector3.Zero, .5f, .28f, .33f, 20);
var torsoBottom = new Box(pelvis.Position + new Vector3(0, .3f, 0), .42f, .48f, .3f, 15);
var torsoTop = new Box(torsoBottom.Position + new Vector3(0, .3f, 0), .5f, .38f, .32f, 20);
var neck = new Box(torsoTop.Position + new Vector3(0, .2f, .04f), .19f, .24f, .2f, 5);
var head = new Sphere(neck.Position + new Vector3(0, .22f, -.04f), .19f, 7);
var leftUpperArm = new Box(torsoTop.Position + new Vector3(-.46f, .1f, 0), .52f, .19f, .19f, 6);
var leftForearm = new Box(leftUpperArm.Position + new Vector3(-.5f, 0, 0), .52f, .18f, .18f, 5);
var leftHand = new Box(leftForearm.Position + new Vector3(-.35f, 0, 0), .28f, .13f, .22f, 4);
var rightUpperArm = new Box(torsoTop.Position + new Vector3(.46f, .1f, 0), .52f, .19f, .19f, 6);
var rightForearm = new Box(rightUpperArm.Position + new Vector3(.5f, 0, 0), .52f, .18f, .18f, 5);
var rightHand = new Box(rightForearm.Position + new Vector3(.35f, 0, 0), .28f, .13f, .22f, 4);
var leftThigh = new Box(pelvis.Position + new Vector3(-.15f, -.4f, 0), .23f, .63f, .23f, 10);
var leftShin = new Box(leftThigh.Position + new Vector3(0, -.6f, 0), .21f, .63f, .21f, 7);
var leftFoot = new Box(leftShin.Position + new Vector3(0, -.35f, -.1f), .23f, .15f, .43f, 5);
var rightThigh = new Box(pelvis.Position + new Vector3(.15f, -.4f, 0), .23f, .63f, .23f, 10);
var rightShin = new Box(rightThigh.Position + new Vector3(0, -.6f, 0), .21f, .63f, .21f, 7);
var rightFoot = new Box(rightShin.Position + new Vector3(0, -.35f, -.1f), .23f, .15f, .43f, 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, .1f, 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 = .05f;
var torsoBottomToTorsoTopBallSocketJoint = new BallSocketJoint(torsoBottom, torsoTop, torsoBottom.Position + new Vector3(0, .25f, 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 = .05f;
var torsoTopToNeckBallSocketJoint = new BallSocketJoint(torsoTop, neck, torsoTop.Position + new Vector3(0, .15f, .05f));
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 = .1f;
var neckToHeadBallSocketJoint = new BallSocketJoint(neck, head, neck.Position + new Vector3(0, .1f, .05f));
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 = .1f;
#endregion
#region Left Arm
var torsoTopToLeftArmBallSocketJoint = new BallSocketJoint(torsoTop, leftUpperArm, torsoTop.Position + new Vector3(-.3f, .1f, 0));
var torsoTopToLeftArmEllipseLimit = new EllipseSwingLimit(torsoTop, leftUpperArm, Vector3.Left, MathHelper.Pi * .75f, 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 = .2f;
var leftUpperArmToLeftForearmSwivelHingeJoint = new SwivelHingeJoint(leftUpperArm, leftForearm, leftUpperArm.Position + new Vector3(-.28f, 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 * .8f;
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 = .3f;
var leftForearmToLeftHandBallSocketJoint = new BallSocketJoint(leftForearm, leftHand, leftForearm.Position + new Vector3(-.2f, 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 = .4f;
#endregion
#region Right Arm
var torsoTopToRightArmBallSocketJoint = new BallSocketJoint(torsoTop, rightUpperArm, torsoTop.Position + new Vector3(.3f, .1f, 0));
var torsoTopToRightArmEllipseLimit = new EllipseSwingLimit(torsoTop, rightUpperArm, Vector3.Right, MathHelper.Pi * .75f, 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 = .2f;
var rightUpperArmToRightForearmSwivelHingeJoint = new SwivelHingeJoint(rightUpperArm, rightForearm, rightUpperArm.Position + new Vector3(.28f, 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 * .8f;
//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 = .3f;
var rightForearmToRightHandBallSocketJoint = new BallSocketJoint(rightForearm, rightHand, rightForearm.Position + new Vector3(.2f, 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 = .4f;
#endregion
#region Left Leg
var pelvisToLeftThighBallSocketJoint = new BallSocketJoint(pelvis, leftThigh, pelvis.Position + new Vector3(-.15f, -.1f, 0));
var pelvisToLeftThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, leftThigh, Vector3.Normalize(new Vector3(-.2f, -1, -.6f)), MathHelper.Pi * .7f, 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 = .1f;
var leftThighToLeftShinRevoluteJoint = new RevoluteJoint(leftThigh, leftShin, leftThigh.Position + new Vector3(0, -.3f, 0), Vector3.Right);
leftThighToLeftShinRevoluteJoint.Limit.IsActive = true;
leftThighToLeftShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8f;
leftThighToLeftShinRevoluteJoint.Limit.MaximumAngle = 0;
leftThighToLeftShinRevoluteJoint.Motor.IsActive = true;
leftThighToLeftShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2f;
var leftShinToLeftFootBallSocketJoint = new BallSocketJoint(leftShin, leftFoot, leftShin.Position + new Vector3(0, -.3f, 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 = .2f;
#endregion
#region Right Leg
var pelvisToRightThighBallSocketJoint = new BallSocketJoint(pelvis, rightThigh, pelvis.Position + new Vector3(.15f, -.1f, 0));
var pelvisToRightThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, rightThigh, Vector3.Normalize(new Vector3(.2f, -1, -.6f)), MathHelper.Pi * .7f, 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 = .1f;
var rightThighToRightShinRevoluteJoint = new RevoluteJoint(rightThigh, rightShin, rightThigh.Position + new Vector3(0, -.3f, 0), Vector3.Right);
rightThighToRightShinRevoluteJoint.Limit.IsActive = true;
rightThighToRightShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8f;
rightThighToRightShinRevoluteJoint.Limit.MaximumAngle = 0;
rightThighToRightShinRevoluteJoint.Motor.IsActive = true;
rightThighToRightShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2f;
var rightShinToRightFootBallSocketJoint = new BallSocketJoint(rightShin, rightFoot, rightShin.Position + new Vector3(0, -.3f, 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 = .2f;
#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 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 = 4000;
NarrowPhaseHelper.Factories.BoxSphere.Count = 1000;
int numColumns = 40;
int numRows = 40;
float xSpacing = 1.0f;
float zSpacing = 1.0f;
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, .2f, 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;
float damping = 20000, stiffness = 20000;
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.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 = .2f;
Space.Add(sphere);
Space.Add(new Box(new Vector3(0, -20.5f, 0), 100f, 10, 100f));
game.Camera.Position = new Vector3(0, 5, 25);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public ReverseTrikeDemo(DemosGame game)
: base(game)
{
game.Camera.Position = new Vector3(0, 2, 15);
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);
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);
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 = 180;
int zLength = 180;
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 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);
}
public void GrabEntity()
{
if (_grabbing)
return;
if (!grabbedEntity.IsDynamic)
return;
EntityTag eT = lastTouched.Tag as EntityTag;
bsj = new BallSocketJoint(grabbedEntity, grabPairCollector, grabPairCollector.Position);
bsj.SpringSettings.StiffnessConstant = 2000000.0f;
nrj = new NoRotationJoint(grabbedEntity, grabPairCollector);
Space.Add(bsj);
Space.Add(nrj);
_grabbing = true;
eT.ParentComponent.Mesh.Highlight = true;
}
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);
}
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 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.5f, 1), new Vector3(-1, 3, 0), 50),
new CompoundShapeEntry(new SphereShape(.45f), new Vector3(.4f, 3, 0), 1),
new CompoundShapeEntry(new SphereShape(.25f), new Vector3(-1.9f, 3.5f, 0), 1),
new CompoundShapeEntry(new SphereShape(.25f), new Vector3(-1.9f, 2.5f, 0), 1),
new CompoundShapeEntry(new SphereShape(.25f), new Vector3(-.3f, 2.3f, 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.4f, 0), .4f, 1.2f, .4f, 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, .7f, 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.StiffnessConstant = 100;
twistLimit.SpringSettings.DampingConstant = 100;
Space.Add(twistLimit);
//Make the lower arm.
Entity lowerArm = new Box(upperArm.Position + new Vector3(0, 1.4f, 0), .35f, 1.3f, .35f, 8);
Space.Add(lowerArm);
var elbow = new SwivelHingeJoint(upperArm, lowerArm, upperArm.Position + new Vector3(0, .6f, 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.DampingConstant = 100;
elbow.TwistLimit.SpringSettings.StiffnessConstant = 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 * .7f;
Space.Add(elbow);
Entity hand = new Box(lowerArm.Position + new Vector3(0, .9f, 0), .4f, .55f, .25f, 3);
Space.Add(hand);
ballSocketJoint = new BallSocketJoint(lowerArm, hand, lowerArm.Position + new Vector3(0, .7f, 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.StiffnessConstant = 100;
twistLimit.SpringSettings.DampingConstant = 100;
Space.Add(twistLimit);
//The hand is pretty floppy without some damping.
var angularMotor = new AngularMotor(lowerArm, hand);
angularMotor.Settings.VelocityMotor.Softness = .5f;
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);
}
void BuildStick(Vector3 position)
{
//Set up a bone chain.
float fullLength = 20;
int linkCount = 20;
float linkLength = fullLength / linkCount;
float linkRadius = linkLength * 0.2f;
var previousBoneEntity = new Cylinder(position, linkLength, linkRadius, 100);
var previousBone = new Bone(previousBoneEntity.Position, previousBoneEntity.Orientation, previousBoneEntity.Radius, previousBoneEntity.Height);
bones.Add(new BoneRelationship(previousBone, previousBoneEntity));
Space.Add(previousBoneEntity);
for (int i = 1; i < linkCount; i++)
{
var boneEntity = new Cylinder(previousBone.Position + new Vector3(0, linkLength, 0), linkLength, linkRadius, 100);
var bone = new Bone(boneEntity.Position, boneEntity.Orientation, boneEntity.Radius, boneEntity.Height);
bones.Add(new BoneRelationship(bone, boneEntity));
Space.Add(boneEntity);
//Make a relationship between the two bones and entities.
CollisionRules.AddRule(previousBoneEntity, boneEntity, CollisionRule.NoBroadPhase);
Vector3 anchor = (previousBoneEntity.Position + boneEntity.Position) / 2;
var dynamicsBallSocketJoint = new BallSocketJoint(previousBoneEntity, boneEntity, anchor);
var dynamicsAngularFriction = new NoRotationJoint(previousBoneEntity, boneEntity);
Space.Add(dynamicsBallSocketJoint);
Space.Add(dynamicsAngularFriction);
var ikBallSocketJoint = new IKBallSocketJoint(previousBone, bone, anchor); //(the joint is auto-added to the bones; no solver-add is needed)
var ikAngularJoint = new IKAngularJoint(previousBone, bone);
joints.Add(ikBallSocketJoint);
joints.Add(ikAngularJoint);
previousBone = bone;
previousBoneEntity = boneEntity;
}
}
void BuildRing(Vector3 position)
{
int incrementCount = 20;
float radius = 5;
float anglePerIncrement = MathHelper.TwoPi / incrementCount;
Bone[] bonesList = new Bone[incrementCount];
for (int i = 0; i < incrementCount; i++)
{
Vector3 bonePosition;
#if !WINDOWS
bonePosition = new Vector3();
#endif
bonePosition.X = (float)Math.Cos(anglePerIncrement * i);
bonePosition.Y = 0;
bonePosition.Z = (float)Math.Sin(anglePerIncrement * i);
bonePosition = bonePosition * radius + position;
bonesList[i] = new Bone(bonePosition,
Quaternion.Concatenate(Quaternion.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2), Quaternion.CreateFromAxisAngle(Vector3.Up, -anglePerIncrement * i)),
0.5f,
MathHelper.Pi * radius * 2 / incrementCount);
}
for (int i = 0; i < bonesList.Length; i++)
{
var boneA = bonesList[i];
var boneB = bonesList[(i + 1) % incrementCount];
var upA = Quaternion.Transform(Vector3.Up, boneA.Orientation);
var upB = Quaternion.Transform(Vector3.Up, boneB.Orientation);
joints.Add(new IKBallSocketJoint(boneA, boneB, (boneA.Position + upA * boneB.HalfHeight + boneB.Position - upB * boneB.HalfHeight) * .5f));
joints.Add(new IKSwingLimit(boneA, boneB, upA, upB, MathHelper.Pi * .5f));
}
Cylinder[] boneEntitiesList = new Cylinder[incrementCount];
for (int i = 0; i < incrementCount; i++)
{
var boneEntity = new Cylinder(new MotionState { Position = bonesList[i].Position, Orientation = bonesList[i].Orientation }, bonesList[i].Height, bonesList[i].Radius, 10);
bones.Add(new BoneRelationship(bonesList[i], boneEntity));
boneEntitiesList[i] = boneEntity;
Space.Add(boneEntity);
}
for (int i = 0; i < incrementCount; i++)
{
var boneA = boneEntitiesList[i];
var boneB = boneEntitiesList[(i + 1) % incrementCount];
var upA = Quaternion.Transform(Vector3.Up, boneA.Orientation);
var upB = Quaternion.Transform(Vector3.Up, boneB.Orientation);
var joint = new BallSocketJoint(boneA, boneB, (boneA.Position + upA * boneB.Height * 0.5f + boneB.Position - upB * boneB.Height * 0.5f) * .5f);
var swingLimit = new SwingLimit(boneA, boneB, upA, upB, MathHelper.Pi * .5f);
Space.Add(swingLimit);
Space.Add(joint);
CollisionRules.AddRule(boneA, boneB, CollisionRule.NoBroadPhase);
}
}
void BuildChain(Vector3 position)
{
//Set up a bone chain.
int linkCount = 100;
var previousBoneEntity = new Cylinder(position, 1, .2f, 10);
var previousBone = new Bone(previousBoneEntity.Position, previousBoneEntity.Orientation, previousBoneEntity.Radius, previousBoneEntity.Height);
bones.Add(new BoneRelationship(previousBone, previousBoneEntity));
Space.Add(previousBoneEntity);
for (int i = 1; i < linkCount; i++)
{
var boneEntity = new Cylinder(previousBone.Position + new Vector3(0, 1, 0), 1, .2f, 10);
var bone = new Bone(boneEntity.Position, boneEntity.Orientation, boneEntity.Radius, boneEntity.Height);
bones.Add(new BoneRelationship(bone, boneEntity));
Space.Add(boneEntity);
//Make a relationship between the two bones and entities.
CollisionRules.AddRule(previousBoneEntity, boneEntity, CollisionRule.NoBroadPhase);
Vector3 anchor = (previousBoneEntity.Position + boneEntity.Position) / 2;
var dynamicsBallSocketJoint = new BallSocketJoint(previousBoneEntity, boneEntity, anchor);
var dynamicsAngularFriction = new AngularMotor(previousBoneEntity, boneEntity);
dynamicsAngularFriction.Settings.Mode = MotorMode.VelocityMotor;
dynamicsAngularFriction.Settings.MaximumForce = 200;
Space.Add(dynamicsBallSocketJoint);
Space.Add(dynamicsAngularFriction);
var ikBallSocketJoint = new IKBallSocketJoint(previousBone, bone, anchor); //(the joint is auto-added to the bones; no solver-add is needed)
joints.Add(ikBallSocketJoint);
previousBone = bone;
previousBoneEntity = boneEntity;
}
}