void AddBackWheel(Vector3 wheelOffset, Entity body)
{
var wheel = new Cylinder(body.Position + wheelOffset, .4m, .5m, 5);
wheel.Material.KineticFriction = 2.5m;
wheel.Material.StaticFriction = 3.5m;
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
var pointOnLineJoint = new PointOnLineJoint(body, wheel, wheel.Position, Vector3.Down, wheel.Position);
var suspensionLimit = new LinearAxisLimit(body, wheel, wheel.Position, wheel.Position, Vector3.Down, -1, 0);
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
var suspensionSpring = new LinearAxisMotor(body, wheel, wheel.Position, wheel.Position, Vector3.Down);
suspensionSpring.Settings.Mode = MotorMode.Servomechanism;
suspensionSpring.Settings.Servo.Goal = 0;
suspensionSpring.Settings.Servo.SpringSettings.Stiffness = 300;
suspensionSpring.Settings.Servo.SpringSettings.Damping = 70;
var revoluteAngularJoint = new RevoluteAngularJoint(body, wheel, Vector3.Right);
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(pointOnLineJoint);
Space.Add(suspensionLimit);
Space.Add(suspensionSpring);
Space.Add(revoluteAngularJoint);
}
public Tread(Entity tankBody, Vector3 offsetToFrontOfTread, int segmentCount, Fix64 spacing, TreadSegmentDescription treadSegmentDescription)
{
Segments = new List <TreadSegment>();
Vector3 nextSegmentPosition = tankBody.Position + offsetToFrontOfTread;
//The front of the tread includes the radius of the first segment.
nextSegmentPosition.Z += treadSegmentDescription.Radius * (Fix64)0.5m;
for (int i = 0; i < segmentCount; ++i)
{
Segments.Add(new TreadSegment(nextSegmentPosition, tankBody, treadSegmentDescription));
//The tread offset starts at the front of the vehicle and moves backward.
nextSegmentPosition.Z += spacing;
}
//Don't let the tread segments rotate relative to each other.
SegmentAngularBindings = new List <NoRotationJoint>();
for (int i = 1; i < segmentCount; ++i)
{
//Create constraints linking the segments together to ensure that the power of one motor is felt by other segments.
SegmentAngularBindings.Add(new NoRotationJoint(Segments[i - 1].Entity, Segments[i].Entity));
//Don't let the tread segments collide.
CollisionRules.AddRule(Segments[i - 1].Entity, Segments[i].Entity, CollisionRule.NoBroadPhase);
}
//Note: You can organize this in different ways. For example, you could have one motor which drives one wheel, which
//in turn drives other wheels through these NoRotationJoints.
//In such a one-motor model, it may be a good idea for stability to bind all wheels directly to the drive wheel with
//NoRotationJoints rather than using a chain of one wheel to the next.
//Per-wheel drive motors are used in this example just because it is slightly more intuitive at a glance.
//Each segment is no different than the others.
}
/// <summary>
/// Constructs a simple character controller.
/// </summary>
/// <param name="position">Location to initially place the character.</param>
/// <param name="characterHeight">The height of the character.</param>
/// <param name="characterWidth">The diameter of the character.</param>
/// <param name="supportHeight">The distance above the ground that the bottom of the character's body floats.</param>
/// <param name="mass">Total mass of the character.</param>
/// <param name="scale">The scale.</param>
public CharacterController(Vector3 position, float characterHeight, float characterWidth, float supportHeight, float mass, Vector3 scale)
{
IsUpdating = false;
characterWidth = characterWidth * scale.X;
this.characterHeight = characterHeight * scale.Y;
this.scale = scale;
Body = new Capsule(position, characterHeight - characterWidth, characterWidth / 2, mass);
collisionPairCollectorPositionOffset = new Vector3(0, -characterHeight / 2 - supportHeight, 0);
collisionPairCollector = new Box(position + collisionPairCollectorPositionOffset, characterWidth, supportHeight * 2, characterWidth, 1);
collisionPairCollector.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhaseUpdate; //Prevents collision detection/contact generation from being run.
collisionPairCollector.IsAffectedByGravity = false;
CollisionRules.AddRule(collisionPairCollector, Body, CollisionRule.NoBroadPhase); //Prevents the creation of any collision pairs between the body and the collector.
rayOriginOffset = new Vector3(0, -characterHeight / 2, 0);
this.supportHeight = supportHeight;
Body.LocalInertiaTensorInverse = new Matrix3X3();
collisionPairCollector.LocalInertiaTensorInverse = new Matrix3X3();
//Make the body slippery.
//Note that this will not make all collisions have zero friction;
//the friction coefficient between a pair of objects is based
//on a blending of the two objects' materials.
Body.Material.KineticFriction = 0;
Body.Material.StaticFriction = 0;
}
public TreadSegment(Vector3 segmentPosition, Entity body, TreadSegmentDescription treadSegmentDescription)
{
Entity = new Cylinder(segmentPosition, treadSegmentDescription.Width, treadSegmentDescription.Radius, treadSegmentDescription.Mass);
Entity.Material.KineticFriction = treadSegmentDescription.Friction;
Entity.Material.StaticFriction = treadSegmentDescription.Friction;
Entity.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(Entity, body, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
SuspensionAxisJoint = new PointOnLineJoint(body, Entity, Entity.Position, Vector3.Down, Entity.Position);
SuspensionLengthLimit = new LinearAxisLimit(body, Entity, Entity.Position, Entity.Position, Vector3.Down, -treadSegmentDescription.SuspensionLength, 0);
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
SuspensionSpring = new LinearAxisMotor(body, Entity, Entity.Position, Entity.Position, Vector3.Down);
SuspensionSpring.Settings.Mode = MotorMode.Servomechanism;
SuspensionSpring.Settings.Servo.Goal = 0;
SuspensionSpring.Settings.Servo.SpringSettings.Stiffness = treadSegmentDescription.SuspensionStiffness;
SuspensionSpring.Settings.Servo.SpringSettings.Damping = treadSegmentDescription.SuspensionDamping;
SuspensionAngularJoint = new RevoluteAngularJoint(body, Entity, Vector3.Right);
//Make the joint extremely rigid. There are going to be extreme conditions when the wheels get up to speed;
//we don't want the forces involved to torque the wheel off the frame!
SuspensionAngularJoint.SpringSettings.Damping *= Entity.Mass * 50;
SuspensionAngularJoint.SpringSettings.Stiffness *= Entity.Mass * 50;
//Motorize the wheel.
Motor = new RevoluteMotor(body, Entity, Vector3.Left);
Motor.Settings.VelocityMotor.Softness = treadSegmentDescription.MotorSoftness;
Motor.Settings.MaximumForce = treadSegmentDescription.MotorMaximumForce;
}
void BuildStick(Vector3 position, int linkCount, out List <Bone> bones, out List <Entity> boneEntities)
{
//Set up a bone chain.
bones = new List <Bone>();
boneEntities = new List <Entity>();
var previousBoneEntity = new Cylinder(position, 1, .2m);
var previousBone = new Bone(previousBoneEntity.Position, previousBoneEntity.Orientation, previousBoneEntity.Radius, previousBoneEntity.Height);
bones.Add(previousBone);
boneEntities.Add(previousBoneEntity);
for (int i = 1; i < linkCount; i++)
{
var boneEntity = new Cylinder(previousBone.Position + new Vector3(0, 1, 0), 1, .2m);
var bone = new Bone(boneEntity.Position, boneEntity.Orientation, boneEntity.Radius, boneEntity.Height);
bones.Add(bone);
boneEntities.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 ballSocket = new IKBallSocketJoint(previousBone, bone, anchor);
var angularJoint = new IKAngularJoint(previousBone, bone);
previousBone = bone;
previousBoneEntity = boneEntity;
}
}
// Must be called by subclasses when they are able to construct an Entity.
protected void SetEntity(Entity entity)
{
if (Entity != null)
{
_physicsSystem.RemoveObject(Entity);
Transform.RemovePhysicsEntity();
}
Entity = entity;
Entity.Position = Transform.Position;
Entity.Orientation = Transform.Rotation;
Entity.IsAffectedByGravity = IsAffectedByGravity;
Entity.PositionUpdateMode = MapUpdateMode(UpdateMode);
Entity.AngularDamping = AngularDamping;
Entity.LinearDamping = LinearDamping;
Entity.Material.Bounciness = Bounciness;
Entity.Material.StaticFriction = StaticFriction;
Entity.Material.KineticFriction = KineticFriction;
Entity.Tag = this;
Entity.CollisionInformation.Tag = this;
Entity.CollisionInformation.CollisionRules.Group = _physicsSystem.GetCollisionGroup(_layer);
if (EnabledInHierarchy)
{
_physicsSystem.AddObject(Entity);
Transform.SetPhysicsEntity(Entity);
if (_isTrigger)
{
SetEntityTrigger();
}
}
_parentCollider = GameObject.GetComponentInParent <Collider>();
if (_parentCollider != null)
{
CollisionRules.AddRule(Entity, _parentCollider.Entity, CollisionRule.NoBroadPhase);
var jointPosition = (Entity.Position + _parentCollider.Entity.Position) / 2;
_parentJoint = new WeldJoint(_parentCollider.Entity, Entity, jointPosition);
_parentJoint.BallSocketJoint.SpringSettings.Stiffness = float.MaxValue;
_parentJoint.BallSocketJoint.SpringSettings.Damping = float.MaxValue;
_parentJoint.BallSocketJoint.IsActive = true;
_parentJoint.NoRotationJoint.SpringSettings.Damping = float.MaxValue;
_parentJoint.NoRotationJoint.SpringSettings.Stiffness = float.MaxValue;
_parentJoint.NoRotationJoint.IsActive = true;
_parentJoint.IsActive = true;
_physicsSystem.AddObject(_parentJoint);
_parentCollider.Transform.PositionManuallyChanged += OnAttachedParentManuallyMoved;
}
}
public void IgnoreCollision(Collider collider1, Collider collider2, bool ignore)
{
if (ignore)
{
CollisionRules.AddRule(collider1.collisionEntity_generic.CollisionInformation, collider2.collisionEntity_generic.CollisionInformation, CollisionRule.NoBroadPhase);
//collider1.collisionEntity_generic.CollisionInformation.CollisionRules.Group = firstStackGroup;
//collider2.collisionEntity_generic.CollisionInformation.CollisionRules.Group = secondStackGroup;
}
else
{
CollisionRules.RemoveRule(collider1.collisionEntity_generic.CollisionInformation, collider2.collisionEntity_generic.CollisionInformation);
//collider1.collisionEntity_generic.CollisionInformation.CollisionRules.Group = null;
//collider2.collisionEntity_generic.CollisionInformation.CollisionRules.Group = null;
}
}
public Turret(Entity tankBody, Vector3 offset)
{
var position = offset + tankBody.Position;
Body = new Cylinder(position, (Fix64)0.7m, (Fix64)0.8m, 8);
//Position the center of the arm a bit further forward since it will be laying down.
position.Z -= 2;
Barrel = new Cylinder(position, 3, (Fix64)0.2m, 3);
//Rotate the arm so that it points straight forward (that is, along {0, 0, -1}).
Barrel.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2);
TankToTurretJoint = new RevoluteJoint(tankBody, Body, Body.Position, Vector3.Up);
TurretBodyToBarrelJoint = new RevoluteJoint(Body, Barrel, Body.Position + new Vector3(0, 0, (Fix64)(-0.5m)), Vector3.Left);
//Turn on the control constraints. We'll put them into servo mode, but velocity mode would also work just fine.
TankToTurretJoint.Motor.IsActive = true;
TankToTurretJoint.Motor.Settings.Mode = MotorMode.Servomechanism;
TankToTurretJoint.Motor.Settings.Servo.BaseCorrectiveSpeed = (Fix64)0.5m;
TankToTurretJoint.Motor.Settings.MaximumForce = 500;
TankToTurretJoint.Motor.Basis.SetLocalAxes(Vector3.Up, Vector3.Forward);
TurretBodyToBarrelJoint.Motor.IsActive = true;
TurretBodyToBarrelJoint.Motor.Settings.Mode = MotorMode.Servomechanism;
TurretBodyToBarrelJoint.Motor.Settings.Servo.BaseCorrectiveSpeed = (Fix64)0.5m;
//We take special care to limit this motor's force to stop the turret from smashing the tank body.
TurretBodyToBarrelJoint.Motor.Settings.MaximumForce = 300;
TurretBodyToBarrelJoint.Motor.Basis.SetLocalAxes(Vector3.Right, Vector3.Forward);
//Don't let the directly connected objects generate collisions. The arm can still hit the tank body, though.
CollisionRules.AddRule(Body, tankBody, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(Barrel, Body, CollisionRule.NoBroadPhase);
//Don't have a lot of leeway downards. The turret will quickly bump the tank body.
MinimumPitch = -MathHelper.Pi * (Fix64)0.05m;
MaximumPitch = MathHelper.Pi * (Fix64)0.4m;
//Build an ammunition pool.
shellPool = new Queue <Sphere>(MaximumShellCount);
for (int i = 0; i < MaximumShellCount; ++i)
{
var shell = new Sphere(new Vector3(10000, 0, 0), (Fix64)0.2m, 2);
shell.PositionUpdateMode = BEPUphysics.PositionUpdating.PositionUpdateMode.Continuous;
shellPool.Enqueue(shell);
}
}
public void OnCollide(object sender, CollisionEventArgs args)
{
if (Stuck)
{
return;
}
double len = GetVelocity().Length();
SetPosition(args.Info.Position + (GetVelocity() / len) * 0.05f);
SetVelocity(Location.Zero);
Gravity = Location.Zero;
if (HasHat)
{
SolidHat = new ModelEntity("invisbox", TheRegion);
SolidHat.SetMass(0);
SolidHat.SetPosition(GetPosition());
SolidHat.SetOrientation(GetOrientation());
SolidHat.scale = new Location(0.6, 1.5, 0.6);
SolidHat.Visible = false;
SolidHat.CanSave = false;
TheRegion.SpawnEntity(SolidHat);
}
if (args.Info.HitEnt != null)
{
PhysicsEntity pe = (PhysicsEntity)args.Info.HitEnt.Tag;
if (pe is EntityDamageable)
{
((EntityDamageable)pe).Damage(Damage + DamageTimesVelocity * (double)len);
}
Vector3 loc = (args.Info.Position - pe.GetPosition()).ToBVector();
Vector3 impulse = GetVelocity().ToBVector() * DamageTimesVelocity / 1000f;
pe.Body.ApplyImpulse(ref loc, ref impulse);
StuckTo = pe;
if (HasHat)
{
CollisionRules.AddRule(pe.Body, SolidHat.Body, CollisionRule.NoBroadPhase); // TODO: Broadcast this info! Perhaps abuse the joint system?
}
}
TheRegion.SendToAll(new PrimitiveEntityUpdatePacketOut(this));
if (args.Info.HitEnt != null)
{
PhysicsEntity pe = (PhysicsEntity)args.Info.HitEnt.Tag;
JointForceWeld jfw = new JointForceWeld(pe, this);
TheRegion.AddJoint(jfw);
}
}
private void onCollision(EntityCollidable sender, Collidable other, BEPUphysics.NarrowPhaseSystems.Pairs.CollidablePairHandler pair)
{
var otherEntity = other as EntityCollidable;
if (otherEntity != null && FollowPlayer && otherEntity == player.character.CharacterController.Body.CollisionInformation)
{
if (!player.canTakeTextures) // we absorbed textures too fast
{
FollowPlayer = false;
Target = targetModel.Entity.Position;
linearMotor.Settings.Servo.Goal = Target;
try
{
CollisionRules.AddRule(Model.Entity, targetModel.Entity, CollisionRule.NoSolver);
}
catch { }
}
else
{
player.giveTexture(this);
}
}
else if (otherEntity != null && !FollowPlayer)
{
if (!targetModel.Texture.Wireframe) // we launched textures too fast. turn around and go back to the player.
{
FollowPlayer = true;
try
{
CollisionRules.AddRule(Model.Entity, player.character.CharacterController.Body, CollisionRule.NoSolver);
}
catch { }
Target = player.character.CharacterController.Body.Position;
linearMotor.Settings.Servo.Goal = Target;
}
else
{
targetModel.GiveTexture(CarriedTex);
player.remove(this);
}
}
}
public Player()
: base(null, null, 100)
{
character = new CharacterControllerInput(GameManager.Space, RenderingDevice.Camera as CharacterCamera, new Vector3(-1, -16, 5));
character.CharacterController.Body.Tag = this;
character.CharacterController.HorizontalMotionConstraint.Speed = 10;
character.CharacterController.HorizontalMotionConstraint.CrouchingSpeed = 5;
PhysicsObject = character.CharacterController.Body; // for posterity
PhysicsObject.CollisionInformation.CollisionRules.Group = dynamicObjects; // also for posterity
rayCastFilter = RayCastFilter;
sword = new BaseModel(delegate { return(Program.Game.Loader.SwordModel); }, false, true, new Vector3(0.5f, -15, 4.2f));
CollisionRules.AddRule(sword.Ent, PhysicsObject, CollisionRule.NoBroadPhase);
sword.Ent.CollisionInformation.LocalPosition = new Vector3(0, -1, 0);
sword.Ent.Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitX, -MathHelper.PiOver2);
swordgrabber = new MotorizedGrabSpring();
swordgrabber.Setup(sword.Ent, sword.ModelPosition - Vector3.UnitX * 0.5f);
sword.Ent.CollisionInformation.Events.PairTouching += onCollision;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRCastingDemo(DemosGame game)
: base(game)
{
bShape = new BoxShape(1, 0, 1);
//bShape.CollisionMargin = 0;
aShape = new ConeShape(1, .4f);
//aShape.CollisionMargin = 0;
a = new Entity(aShape);
b = new Entity(bShape);
CollisionRules.AddRule(a, b, CollisionRule.NoSolver);
NarrowPhaseHelper.CollisionManagers.Remove(new TypePair(typeof(ConvexCollidable <BoxShape>), typeof(ConvexCollidable <BoxShape>)));
Space.Add(a);
Space.Add(b);
a.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver4);
b.Orientation = Quaternion.Identity;
aTransform = new RigidTransform(new Vector3(-10, -10, -10), a.Orientation);
bTransform = new RigidTransform(new Vector3(10, 10, 10), b.Orientation);
game.Camera.Position = new Vector3(0, 5, 17);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public FishInABarrelDemo(DemosGame game)
: base(game)
{
game.Camera.Position = new Vector3(0, 7, 30);
var detector = new Box(new Vector3(0, 0, 0), 1.5f, 1.5f, 1.5f);
detector.CollisionInformation.CollisionRules.Personal = CollisionRule.NoSolver;
var acceptedTriggerEntity = new Box(new Vector3(5, 0, 0), 1.6f, .7f, .4f, 1);
acceptedTrigger = acceptedTriggerEntity.CollisionInformation;
detector.Tag = "noDisplayObject";
acceptedTriggerEntity.Tag = "noDisplayObject";
Space.Add(detector);
Space.Add(acceptedTriggerEntity);
var fish = game.Content.Load <Model>("fish");
game.ModelDrawer.Add(new DisplayEntityModel(acceptedTriggerEntity, fish, game.ModelDrawer));
var barrelAndPlatform = game.Content.Load <Model>("barrelAndPlatform");
Vector3[] staticTriangleVertices;
int[] staticTriangleIndices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(barrelAndPlatform, out staticTriangleVertices, out staticTriangleIndices);
//Note that the final 'margin' parameter is optional, but can be used to specify a collision margin on triangles in the static triangle group.
var fishDepositoryGroup = new StaticMesh(staticTriangleVertices, staticTriangleIndices);
CollisionRules.AddRule(fishDepositoryGroup, detector, CollisionRule.NoBroadPhase);
Space.Add(fishDepositoryGroup);
game.ModelDrawer.Add(fishDepositoryGroup);
movedBox = new Box(new Vector3(-4, 5, 0), 1, 1, 1, 1);
detector.Space.Add(movedBox);
detector.CollisionInformation.Events.InitialCollisionDetected += InitialCollisionDetected;
detector.CollisionInformation.Events.CollisionEnded += CollisionEnded;
}
public Entity AddWheel(Vector3 wheelPosition, Entity baseBody)
{
BepuEntity wheel = new BepuEntity();
wheel.modelName = "cyl";
wheel.LoadContent();
wheel.body = new Cylinder(wheelPosition, 2, 2, 2);
wheel.localTransform = Matrix.CreateScale(2f, 2f, 2f);
wheel.body.Material.KineticFriction = 2.5f;
wheel.body.Material.StaticFriction = 2.5f;
wheel.body.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
wheel.diffuse = new Vector3(0, 0, 0);
//Prevents collisionf from happening
CollisionRules.AddRule(wheel.body, baseBody, CollisionRule.NoBroadPhase);
//Add the wheel and connection to the space.
Game1.Instance.Space.Add(wheel.body);
Game1.Instance.Children.Add(wheel);
return(wheel.body);
}
/// <summary>
/// Constructs a simple character controller.
/// </summary>
/// <param name="position">Location to initially place the character.</param>
/// <param name="characterHeight">The height of the character.</param>
/// <param name="characterWidth">The diameter of the character.</param>
/// <param name="mass">Total mass of the character.</param>
/// <param name="maximumStepHeight">Height that the character can climb up.</param>
public CharacterControllerOld(Vector3 position, float characterHeight, float characterWidth, float mass, float maximumStepHeight)
{
//Create the physical body of the character.
//The character's cylinder height and radius must be shrunk down marginally
//to take into account the collision margin and support margin while still fitting in the defined character height/width.
var bodyPosition = new Vector3(position.X, position.Y + supportMargin / 2, position.Z);
float collisionMargin = .04f;
Body = new Cylinder(bodyPosition,
characterHeight - 2 * collisionMargin - supportMargin,
characterWidth / 2 - collisionMargin,
mass);
Body.CollisionInformation.Shape.CollisionMargin = collisionMargin;
feetCollisionPairCollectorPositionOffset = new Vector3(0, -Body.Height / 2 - supportMargin - collisionMargin, 0);
feetCollisionPairCollector = new Box(bodyPosition + feetCollisionPairCollectorPositionOffset, characterWidth, maximumStepHeight * 2, characterWidth, 1);
feetCollisionPairCollector.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhaseUpdate; //Prevents collision detection/contact generation from being run.
feetCollisionPairCollector.IsAffectedByGravity = false;
CollisionRules.AddRule(feetCollisionPairCollector, Body, CollisionRule.NoBroadPhase); //Prevents the creation of any collision pairs between the body and the collector.
feetSupportFinderOffset = new Vector3(0, feetCollisionPairCollectorPositionOffset.Y + maximumStepHeight, 0);
headCollisionPairCollectorPositionOffset = new Vector3(0, (Body.Height + maximumStepHeight) / 2 + collisionMargin, 0);
headCollisionPairCollector = new Box(bodyPosition + headCollisionPairCollectorPositionOffset, characterWidth, maximumStepHeight + collisionMargin, characterWidth, 1);
headCollisionPairCollector.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhaseUpdate; //Prevents collision detection/contact generation from being run.
headCollisionPairCollector.IsAffectedByGravity = false;
CollisionRules.AddRule(headCollisionPairCollector, Body, CollisionRule.NoBroadPhase); //Prevents the creation of any collision pairs between the body and the collector.
headBlockageFinderOffset = new Vector3(0, headCollisionPairCollectorPositionOffset.Y - maximumStepHeight / 2 - collisionMargin, 0);
castingShape = new CylinderShape(0, Body.Radius + collisionMargin);
castingShape.CollisionMargin = 0;
this.maximumStepHeight = maximumStepHeight;
this.supportMargin = .01f;
Body.LocalInertiaTensorInverse = new Matrix3X3();
feetCollisionPairCollector.LocalInertiaTensorInverse = new Matrix3X3();
headCollisionPairCollector.LocalInertiaTensorInverse = new Matrix3X3();
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public CollisionFilteringDemo(DemosGame game)
: base(game)
{
Entity toAdd;
toAdd = new Box(new Vector3(0, -.5f, 0), 50, 1, 50);
Space.Add(toAdd);
//Set up two stacks which go through each other
var firstStackGroup = new CollisionGroup();
var secondStackGroup = new CollisionGroup();
//Adding this rule to the space's collision group rules will prevent entities belong to these two groups from generating collision pairs with each other.
groupPair = new CollisionGroupPair(firstStackGroup, secondStackGroup);
CollisionRules.CollisionGroupRules.Add(groupPair, CollisionRule.NoBroadPhase);
for (int k = 0; k < 10; k++)
{
toAdd = new Box(
new Vector3(-4 + .12f * k, .5f + k, 0), 1f, 1f, 1f,
10);
toAdd.CollisionInformation.CollisionRules.Group = firstStackGroup;
Space.Add(toAdd);
toAdd = new Box(new Vector3(4 - .12f * k, .5f + k, 0),
1f, 1f, 1f, 10);
toAdd.CollisionInformation.CollisionRules.Group = secondStackGroup;
Space.Add(toAdd);
}
//Add another two boxes which ignore each other using the specific entities method; they will still collide with the stacks since they will have the default dynamic collision group.
toAdd = new Box(new Vector3(1, 3, 0), 1f, 4f, 2f, 10);
var toAdd2 = new Box(new Vector3(-1, 3, 0), 1f, 4f, 2f, 15);
CollisionRules.AddRule(toAdd, toAdd2, CollisionRule.NoBroadPhase);
Space.Add(toAdd);
Space.Add(toAdd2);
game.Camera.Position = new Microsoft.Xna.Framework.Vector3(0, 6, 20);
}
void AddBackWheel(Vector3 suspensionOffset, Entity body, bool leftSide)
{
var suspensionLeg = new Box(body.Position + suspensionOffset, 0.25f, 0.8f, 0.25f, 10);
const float horizontalWheelOffset = 0.2f;
var wheel = new Cylinder(suspensionLeg.Position + new Vector3(leftSide ? -horizontalWheelOffset : horizontalWheelOffset, -suspensionLeg.HalfHeight, 0), .2f, .3f, 5f);
wheel.Material.KineticFriction = 2.5f;
wheel.Material.StaticFriction = 3.5f;
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(wheel, suspensionLeg, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(suspensionLeg, body, CollisionRule.NoBroadPhase);
//Connect the suspension to the body.
var bodyToSuspension = new PrismaticJoint(body, suspensionLeg, suspensionLeg.Position, Vector3.Down, suspensionLeg.Position);
bodyToSuspension.Motor.Settings.Mode = MotorMode.Servomechanism;
bodyToSuspension.Motor.IsActive = true;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Stiffness = 300;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Damping = 70;
bodyToSuspension.Limit.IsActive = true;
bodyToSuspension.Limit.Minimum = -0.5f;
bodyToSuspension.Limit.Maximum = 0;
//Connect the wheel to the suspension.
var suspensionToWheel = new RevoluteJoint(suspensionLeg, wheel, wheel.Position, Vector3.Right);
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(suspensionLeg);
Space.Add(bodyToSuspension);
Space.Add(suspensionToWheel);
}
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
}
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 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 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 MobileMeshSolidityTestDemo(DemosGame game)
: base(game)
{
Vector3[] vertices;
int[] indices;
//Hardcoded box
vertices = new Vector3[]
{
new Vector3(0.5m, 0.5m, 0.5m),
new Vector3(0.5m, 0.5m, -0.5m),
new Vector3(-0.5000001m, 0.5m, -0.4999999m),
new Vector3(-0.4999998m, 0.5m, 0.5000002m),
new Vector3(-0.4999998m, -0.5m, 0.5000002m),
new Vector3(-0.5000001m, -0.5m, -0.4999999m),
new Vector3(0.5m, -0.5m, -0.5m),
new Vector3(0.5m, -0.5m, 0.5m),
new Vector3(0.5m, 0.5m, 0.5m),
new Vector3(0.5m, -0.5m, 0.5m),
new Vector3(0.5m, -0.5m, -0.5m),
new Vector3(0.5m, 0.5m, -0.5m),
new Vector3(0.5m, 0.5m, -0.5m),
new Vector3(0.5m, -0.5m, -0.5m),
new Vector3(-0.5000001m, -0.5m, -0.4999999m),
new Vector3(-0.5000001m, 0.5m, -0.4999999m),
new Vector3(-0.5000001m, 0.5m, -0.4999999m),
new Vector3(-0.5000001m, -0.5m, -0.4999999m),
new Vector3(-0.4999998m, -0.5m, 0.5000002m),
new Vector3(-0.4999998m, 0.5m, 0.5000002m),
new Vector3(-0.4999998m, 0.5m, 0.5000002m),
new Vector3(-0.4999998m, -0.5m, 0.5000002m),
new Vector3(0.5m, -0.5m, 0.5m),
new Vector3(0.5m, 0.5m, 0.5m)
};
indices = new[]
{
2, 1, 0,
3, 2, 0,
6, 5, 4,
7, 6, 4,
10, 9, 8,
11, 10, 8,
14, 13, 12,
15, 14, 12,
18, 17, 16,
19, 18, 16,
22, 21, 20,
23, 22, 20
};
var mesh = new MobileMesh(vertices, indices, AffineTransform.Identity, MobileMeshSolidity.Solid, 10);
Space.Add(mesh);
//Tube
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("tube"), out vertices, out indices);
mesh = new MobileMesh(vertices, indices, AffineTransform.Identity, MobileMeshSolidity.Solid, 10);
mesh.Position = new Vector3(-10, 10, 0);
Space.Add(mesh);
//Cube
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("cube"), out vertices, out indices);
mesh = new MobileMesh(vertices, indices, AffineTransform.Identity, MobileMeshSolidity.Solid, 10);
mesh.Position = new Vector3(10, 0, 0);
Space.Add(mesh);
//Guy
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("guy"), out vertices, out indices);
mesh = new MobileMesh(vertices, indices, AffineTransform.Identity, MobileMeshSolidity.Solid, 10);
mesh.Position = new Vector3(0, 0, 10);
Space.Add(mesh);
//Barrel Platform
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("barrelandplatform"), out vertices, out indices);
mesh = new MobileMesh(vertices, indices, AffineTransform.Identity, MobileMeshSolidity.Solid, 10);
mesh.Position = new Vector3(0, 0, -10);
Space.Add(mesh);
//FloaterTube
ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load <Model>("tube"), out vertices, out indices);
mesh = new MobileMesh(vertices, indices, new AffineTransform(new Vector3(1, 1, 1), Quaternion.Identity, new Vector3(0, 0, 0)), MobileMeshSolidity.Solid);
mesh.Position = new Vector3(5, 18, 0);
Space.Add(mesh);
//Float a box through the last mesh to check contact generation controllably.
var solidityTester = new Box(new Vector3(5, 8, 0), 1, 1, 1);
solidityTester.LinearVelocity = new Vector3(0, 1, 0);
CollisionRules.AddRule(solidityTester, mesh, CollisionRule.NoSolver);
Space.Add(solidityTester);
Space.Add(new Box(new Vector3(0, -5, 0), 50, 1, 50));
game.Camera.Position = new Vector3(0, 10, 20);
}
/// <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 void ApplyHook(PlayerEntity player, ItemStack item, Location Position, BEPUphysics.Entities.Entity HitEnt)
{
RemoveHook(player);
PhysicsEntity pe;
double len = (double)(Position - player.GetCenter()).Length();
Location step = (player.GetCenter() - Position) / len;
Location forw = Utilities.VectorToAngles(step);
forw.Yaw += 180;
BEPUutilities.Quaternion quat = BEPUutilities.Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), (double)(forw.Pitch * Utilities.PI180)) *
BEPUutilities.Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), (double)(forw.Yaw * Utilities.PI180));
if (HitEnt == null)
{
ModelEntity mod = new ModelEntity("cube", player.TheRegion)
{
Mass = 0,
CanSave = false,
scale = new Location(0.023, 0.05, 0.05),
mode = ModelCollisionMode.AABB
};
mod.SetPosition(Position);
mod.SetOrientation(quat);
player.TheRegion.SpawnEntity(mod);
pe = mod;
player.Hooks.Add(new HookInfo()
{
Joint = null, Hit = pe, IsBar = true
});
}
else
{
pe = (PhysicsEntity)HitEnt.Tag;
}
JointDistance jd;
//jd = new JointDistance(player, pe, 0.01f, len + 0.01f, player.GetCenter(), Position);
//player.TheRegion.AddJoint(jd);
//player.Hooks.Add(new HookInfo() { Joint = jd, Hit = pe, IsBar = false });
PhysicsEntity cent = pe;
for (double f = 0; f < len - 1f; f += 0.5f)
{
Location cpos = Position + step * f;
ModelEntity ce = new ModelEntity("cube", player.TheRegion)
{
Mass = 15,
CanSave = false,
scale = new Location(0.023, 0.05, 0.05),
mode = ModelCollisionMode.AABB
};
ce.SetPosition(cpos + step * 0.5);
ce.SetOrientation(quat);
player.TheRegion.SpawnEntity(ce);
jd = new JointDistance(ce, cent, 0.01f, 0.5f, ce.GetPosition(), (ReferenceEquals(cent, pe) ? Position: cent.GetPosition()));
CollisionRules.AddRule(player.Body, ce.Body, CollisionRule.NoBroadPhase);
player.TheRegion.AddJoint(jd);
player.Hooks.Add(new HookInfo()
{
Joint = jd, Hit = ce, IsBar = true
});
cent = ce;
}
jd = new JointDistance(cent, player, 0.01f, 0.5f, cent.GetPosition(), player.GetCenter());
player.TheRegion.AddJoint(jd);
player.Hooks.Add(new HookInfo()
{
Joint = jd, Hit = player, IsBar = false
});
}
public override void Enable()
{
CollisionRules.AddRule(((PhysicsEntity)One).Body.CollisionInformation, ((PhysicsEntity)Two).Body.CollisionInformation, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(((PhysicsEntity)Two).Body.CollisionInformation, ((PhysicsEntity)One).Body.CollisionInformation, CollisionRule.NoBroadPhase);
base.Enable();
}
/// <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 simple character controller.
/// </summary>
/// <param name="position">Location to initially place the character.</param>
/// <param name="height">The height of the character.</param>
/// <param name="radius">The diameter of the character.</param>
/// <param name="supportHeight">The distance above the ground that the bottom of the character's body floats.</param>
/// <param name="mass">Total mass of the character.</param>
public Character(Main main, Bindable bindable, Vector3 position, float height = Character.DefaultHeight, float crouchedHeight = Character.DefaultCrouchedHeight, float radius = Character.DefaultRadius, float supportHeight = Character.DefaultSupportHeight, float crouchedSupportHeight = Character.DefaultCrouchedSupportHeight, float mass = Character.DefaultMass)
{
this.main = main;
this.Radius.Value = radius;
this.Mass.Value = mass;
this.Body = new Capsule(position, height, radius, mass);
this.Body.Tag = this;
this.Body.CollisionInformation.Tag = this;
this.Body.IgnoreShapeChanges = true;
this.Body.LinearDamping = 0.0f;
this.Body.CollisionInformation.CollisionRules.Group = Character.CharacterGroup;
this.NormalHeight = height;
this.CrouchedHeight = crouchedHeight;
this.Body.CollisionInformation.Events.ContactCreated += new BEPUphysics.BroadPhaseEntries.Events.ContactCreatedEventHandler <EntityCollidable>(Events_ContactCreated);
this.collisionPairCollector = new Box(position + new Vector3(0, (height * -0.5f) - supportHeight, 0), radius * 2, supportHeight * 2, radius, 1);
this.collisionPairCollector.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhaseUpdate; //Prevents collision detection/contact generation from being run.
this.collisionPairCollector.IsAffectedByGravity = false;
this.collisionPairCollector.CollisionInformation.CollisionRules.Group = Character.CharacterGroup;
CollisionRules.AddRule(this.collisionPairCollector, this.Body, CollisionRule.NoBroadPhase); //Prevents the creation of any collision pairs between the body and the collector.
this.SupportHeight.Value = supportHeight;
this.NormalSupportHeight = supportHeight;
this.CrouchedSupportHeight = crouchedSupportHeight;
this.Body.LocalInertiaTensorInverse = new BEPUutilities.Matrix3x3();
this.collisionPairCollector.LocalInertiaTensorInverse = new BEPUutilities.Matrix3x3();
bindable.Add(new ChangeBinding <bool>(this.Crouched, delegate(bool old, bool value)
{
if (value && !old)
{
this.Body.Position += new Vector3(0, (this.CrouchedSupportHeight - this.NormalSupportHeight) + 0.5f * (this.CrouchedHeight - this.NormalHeight), 0);
this.Height.Value = this.CrouchedHeight;
this.Body.Length = this.Height.Value - this.Radius * 2;
this.SupportHeight.Value = this.CrouchedSupportHeight;
}
else if (!value && old)
{
this.Height.Value = this.NormalHeight;
this.Body.Length = this.Height.Value - this.Radius * 2;
this.Body.Position += new Vector3(0, (this.NormalSupportHeight - this.CrouchedSupportHeight) + 0.5f * (this.NormalHeight - this.CrouchedHeight), 0);
this.SupportHeight.Value = this.NormalSupportHeight;
}
this.collisionPairCollector.Height = this.SupportHeight * 2;
this.Transform.Value = this.Body.WorldTransform;
}));
bindable.Add(new SetBinding <Matrix>(this.Transform, delegate(Matrix m)
{
this.Body.WorldTransform = m;
}));
bindable.Add(new SetBinding <Vector3>(this.LinearVelocity, delegate(Vector3 v)
{
this.Body.LinearVelocity = v;
}));
//Make the body slippery.
//Note that this will not make all collisions have zero friction;
//the friction coefficient between a pair of objects is based
//on a blending of the two objects' materials.
this.Body.Material.KineticFriction = 0.0f;
this.Body.Material.StaticFriction = 0.0f;
this.Body.Material.Bounciness = 0.0f;
const int rayChecks = 4;
float rayCheckRadius = radius - 0.1f;
this.rayOffsets = new[] { Vector3.Zero }.Concat(Enumerable.Range(0, rayChecks).Select(
delegate(int x)
{
float angle = x * ((2.0f * (float)Math.PI) / (float)rayChecks);
return(new Vector3((float)Math.Cos(angle) * rayCheckRadius, 0, (float)Math.Sin(angle) * rayCheckRadius));
})).ToArray();
this.IsUpdating = false;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRTestDemo(DemosGame game)
: base(game)
{
var shapeA = new BoxShape(1, 1, 1);
shapeA.CollisionMargin = 0;
var shapeB = new BoxShape(1, 1, 1);
shapeB.CollisionMargin = 0;
var transformA = new RigidTransform(new Vector3(0, 0, 0));
var transformB = new RigidTransform(new Vector3(.5m, .5m, 0));
Vector3 overlap;
bool overlapped = MPRToolbox.GetLocalOverlapPosition(shapeA, shapeB, ref transformB, out overlap);
Vector3 normal;
Fix64 depth;
Vector3 direction = new Vector3(0, -1, 0);
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref transformB, ref direction, out depth, out normal);
ContactData contactData;
//bool overlappedOld = MPRToolboxOld.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
//Random rand = new Random(0);
//for (int i = 0; i < 10000000; i++)
//{
// transformA = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// transformB = new RigidTransform(new Vector3((Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5, (Fix64)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000, (Fix64)rand.NextDouble() * 1000));
// overlapped = MPRTesting.GetOverlapPosition(shapeA, shapeB, ref transformA, ref transformB, out overlap);
// overlappedOld = MPRToolbox.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
// if (overlapped && !overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlappedOld && !overlapped &&
// (!MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlapped && overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
//}
//Do these tests with rotationally immobile objects.
CollisionDetectionSettings.DefaultMargin = 0;
groundWidth = 10;
groundHeight = .1m;
groundLength = 10;
//a = new Box(new Vector3(0, -5, 0), groundWidth, groundHeight, groundLength, 1);
//a = new TransformableEntity(new Vector3(0,0,0), new TriangleShape(new Vector3(-5, -5, -5), new Vector3(5, -5, -5), new Vector3(-5, -5, 5)), Matrix3x3.Identity);
a = new Triangle(new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), 1);
Space.Add(a);
Space.ForceUpdater.Gravity = new Vector3();
boxWidth = .25m;
boxHeight = .05m;
boxLength = 1;
b = new TransformableEntity(new Vector3(0, 2, 0), new BoxShape(boxWidth, boxHeight, boxLength), Matrix3x3.Identity, 1);
//b = new Cone(new Vector3(0, 2, 0), .2m, .1m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5m, 1);
b.LocalInertiaTensorInverse = new Matrix3x3();
CollisionRules.AddRule(b, a, CollisionRule.NoSolver);
b.ActivityInformation.IsAlwaysActive = true;
Space.Add(b);
//Space.Add(new TransformableEntity(new Vector3(0, 4, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Space.Add( new TransformableEntity(new Vector3(0, 6, 0), new BoxShape(1, 1, 1), Matrix3x3.Identity, 1));
//Vector3[] vertices = new Vector3[] { new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), new Vector3(0, -60, 5) };
//int[] indices = new int[] { 0, 1, 2 , 0, 2, 3 };
//StaticMesh mesh = new StaticMesh(vertices, indices);
//Space.Add(mesh);
//mesh.ImproveBoundaryBehavior = true;
//mesh.Sidedness = TriangleSidedness.Counterclockwise;
//game.ModelDrawer.Add(mesh);
//mesh.CollisionRules.Personal = CollisionRule.NoSolver;
}
void AddDriveWheel(Vector3 suspensionOffset, Entity body, bool leftSide, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor, out Box suspensionLeg)
{
suspensionLeg = new Box(body.Position + suspensionOffset, 0.25f, 0.8f, 0.25f, 10);
const float horizontalWheelOffset = 0.2f;
var wheel = new Cylinder(suspensionLeg.Position + new Vector3(leftSide ? -horizontalWheelOffset : horizontalWheelOffset, -suspensionLeg.HalfHeight, 0), .2f, .3f, 5f);
wheel.Material.KineticFriction = 2.5f;
wheel.Material.StaticFriction = 3.5f;
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(wheel, suspensionLeg, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(suspensionLeg, body, CollisionRule.NoBroadPhase);
//Connect the suspension to the body.
var bodyToSuspension = new LineSliderJoint(body, suspensionLeg, suspensionLeg.Position, Vector3.Down, suspensionLeg.Position);
bodyToSuspension.Limit.IsActive = true;
bodyToSuspension.Limit.Minimum = -0.5f;
bodyToSuspension.Limit.Maximum = 0;
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
bodyToSuspension.Motor.IsActive = true;
bodyToSuspension.Motor.Settings.Mode = MotorMode.Servomechanism;
bodyToSuspension.Motor.Settings.Servo.Goal = 0;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Stiffness = 300;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Damping = 70;
steeringMotor = new RevoluteMotor(body, suspensionLeg, Vector3.Up);
steeringMotor.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.
steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
steeringMotor.TestAxis = Vector3.Right;
//To make the steering a little more responsive, set a base speed at which error gets corrected.
//This works on top of the default error reduction implied by the constraint's spring constants.
steeringMotor.Settings.Servo.BaseCorrectiveSpeed = 1;
//The revolute motor is weaker than some other types of constraints and maintaining a goal in the presence of extremely fast rotation and integration issues.
//Laying a revolute limit on top of it can help mitigate the problem.
var steeringConstraint = new RevoluteLimit(body, suspensionLeg, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);
//Connect the wheel to the suspension.
var suspensionToWheel = new RevoluteJoint(suspensionLeg, wheel, wheel.Position, Vector3.Right);
drivingMotor = suspensionToWheel.Motor;
//The driving motor's default, created by the RevoluteJoint constructor above, chose the axis of rotation such that negatives values made the car go forward and vice versa.
//Swap it around so that the positive values make the car roll forward instead!
drivingMotor.Basis.SetWorldAxes(Vector3.Left, Vector3.Forward);
drivingMotor.TestAxis = Vector3.Forward;
drivingMotor.Settings.VelocityMotor.Softness = .3f;
drivingMotor.Settings.MaximumForce = 100;
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(suspensionLeg);
Space.Add(bodyToSuspension);
Space.Add(drivingMotor);
Space.Add(steeringMotor);
Space.Add(steeringConstraint);
Space.Add(suspensionToWheel);
}