public FixedOffsetCameraControlScheme(Entity entity, Camera camera, DemosGame game)
: base(camera, game)
{
Entity = entity;
UseCameraSmoothing = true;
CameraOffset = new Vector3(0, 0.7f, 0);
}
/// <summary>
/// Constructs a thruster originating at the given position, pushing in the given direction.
/// </summary>
/// <param name="targetEntity">Entity that the force will be applied to.</param>
/// <param name="pos">Origin of the force.</param>
/// <param name="dir">Direction of the force.</param>
/// <param name="time">Total lifespan of the force. A lifespan of zero is infinite.</param>
public Thruster(Entity targetEntity, Vector3 pos, Vector3 dir, float time)
{
Target = targetEntity;
Position = pos;
Direction = dir;
LifeSpan = time;
}
/// <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;
}
/// <summary>
/// Constructs a new display model.
/// </summary>
/// <param name="entity">Entity to follow.</param>
/// <param name="model">Model to draw on the entity.</param>
/// <param name="modelDrawer">Model drawer to use.</param>
public DisplayEntityModel(Entity entity, Model model, ModelDrawer modelDrawer)
: base(modelDrawer)
{
OffsetTransform = Matrix.Identity;
Entity = entity;
Model = model;
}
/// <summary>
/// Maintains the position of the character's body above the ground.
/// </summary>
/// <param name="supportLocationVelocity">Velocity of the support point connected to the supportEntity.</param>
/// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
/// <param name="supportDistance">Distance from the character to the support location.</param>
private void support(Vector3 supportLocationVelocity, Vector3 supportNormal, float supportDistance, float dt)
{
//Put the character at the right distance from the ground.
float supportVerticalVelocity = Math.Max(supportLocationVelocity.Y, -0.1f);
float heightDifference = this.SupportHeight - supportDistance;
this.Body.Position += (new Vector3(0, MathHelper.Clamp(heightDifference, (supportVerticalVelocity - 10.0f) * dt, (supportVerticalVelocity + 10.0f) * dt), 0));
//Remove from the character velocity which would push it toward or away from the surface.
//This is a relative velocity, so the velocity of the body and the velocity of a point on the support entity must be found.
float bodyNormalVelocity = Vector3.Dot(this.Body.LinearVelocity, supportNormal);
float supportEntityNormalVelocity = Vector3.Dot(supportLocationVelocity, supportNormal);
Vector3 diff = (bodyNormalVelocity - supportEntityNormalVelocity) * -supportNormal;
diff.Y = Math.Max(diff.Y, 0);
this.Body.LinearVelocity += diff;
BEPUphysics.Entities.Entity supportEntity = this.SupportEntity;
if (supportEntity != null && supportEntity.IsAffectedByGravity)
{
Vector3 supportLocation = this.SupportLocation;
Vector3 impulse = (this.Body.Mass * 1.5f) * ((Space)this.Space).ForceUpdater.Gravity * dt;
supportEntity.ApplyImpulse(ref supportLocation, ref impulse);
supportEntity.ActivityInformation.Activate();
}
}
/// <summary>
/// Locates the closest support entity by performing a raycast at collected candidates.
/// </summary>
/// <param name="supportEntity">The closest supporting entity.</param>
/// <param name="supportLocation">The support location where the ray hit the entity.</param>
/// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
/// <param name="supportDistance">Distance from the character to the support location.</param>
/// <returns>Whether or not a support was located.</returns>
private bool FindSupport(out BEPUphysics.Entities.Entity supportEntity, out Vector3 supportLocation, out Vector3 supportNormal, out float supportDistance)
{
supportEntity = null;
supportLocation = Toolbox.NoVector;
supportNormal = Toolbox.NoVector;
supportDistance = float.MaxValue;
Vector3 rayOrigin = Body.Position + rayOriginOffset;
for (int i = 0; i < collisionPairCollector.CollisionInformation.Pairs.Count; i++)
{
var pair = collisionPairCollector.CollisionInformation.Pairs[i];
//Determine which member of the collision pair is the possible support.
Collidable candidate = (pair.BroadPhaseOverlap.EntryA == collisionPairCollector.CollisionInformation ? pair.BroadPhaseOverlap.EntryB : pair.BroadPhaseOverlap.EntryA) as Collidable;
//Ensure that the candidate is a valid supporting entity.
if (candidate.CollisionRules.Personal >= CollisionRule.NoSolver)
{
continue; //It is invalid!
}
//The maximum length is supportHeight * 2 instead of supportHeight alone because the character should be able to step downwards.
//This acts like a sort of 'glue' to help the character stick on the ground in general.
float maximumDistance;
//The 'glue' effect should only occur if the character has a solid hold on the ground though.
//Otherwise, the character is falling or sliding around uncontrollably.
if (HasTraction)
{
maximumDistance = supportHeight * 2;
}
else
{
maximumDistance = supportHeight;
}
RayHit rayHit;
//Fire a ray at the candidate and determine some details!
if (candidate.RayCast(new Ray(rayOrigin, Vector3.Down), maximumDistance, out rayHit))
{
//We want to find the closest support, so compare it against the last closest support.
if (rayHit.T < supportDistance)
{
supportDistance = rayHit.T;
supportLocation = rayHit.Location;
supportNormal = rayHit.T > 0 ? rayHit.Normal : Vector3.Up;
var entityInfo = candidate as EntityCollidable;
if (entityInfo != null)
{
supportEntity = entityInfo.Entity;
}
else
{
supportEntity = null;
}
}
}
}
supportNormal.Normalize();
return(supportDistance < float.MaxValue);
}
/// <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 point on plane constraint.
/// </summary>
/// <param name="connectionA">Entity to which the constraint's plane is attached.</param>
/// <param name="connectionB">Entity to which the constraint's point is attached.</param>
/// <param name="planeAnchor">A point on the plane.</param>
/// <param name="normal">Direction, attached to the first connected entity, defining the plane's normal</param>
/// <param name="pointAnchor">The point to constrain to the plane, attached to the second connected object.</param>
public PointOnPlaneJoint(Entity connectionA, Entity connectionB, Vector3 planeAnchor, Vector3 normal, Vector3 pointAnchor)
{
ConnectionA = connectionA;
ConnectionB = connectionB;
PointAnchor = pointAnchor;
PlaneAnchor = planeAnchor;
PlaneNormal = normal;
}
/// <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);
}
public void refreshModel(GLContext context)
{
if (model != pmodel)
{
pmodel = model;
internalModel = context.Models.GetModel(model);
if (mode == ModelCollisionMode.PRECISE)
{
mesh = context.Models.Handler.MeshToBepu(internalModel.OriginalModel);
offset = -Location.FromBVector(mesh.Position);
}
else if (mode == ModelCollisionMode.AABB)
{
List <BEPUutilities.Vector3> vecs = context.Models.Handler.GetCollisionVertices(internalModel.OriginalModel);
Location zero = new Location(vecs[0].X, vecs[0].Y, vecs[0].Z);
AABB abox = new AABB()
{
Min = zero, Max = zero
};
for (int v = 1; v < vecs.Count; v++)
{
abox.Include(new Location(vecs[v].X, vecs[v].Y, vecs[v].Z));
}
Location size = abox.Max - abox.Min;
offset = abox.Max - size / 2;
mesh = new BEPUphysics.Entities.Prefabs.Box(
new BEPUphysics.EntityStateManagement.MotionState()
{
Position = Position.ToBVector(), Orientation = Angle
},
(float)size.X, (float)size.Y, (float)size.Z);
}
else
{
List <BEPUutilities.Vector3> vecs = context.Models.Handler.GetCollisionVertices(internalModel.OriginalModel);
double distSq = 0;
for (int v = 1; v < vecs.Count; v++)
{
if (vecs[v].LengthSquared() > distSq)
{
distSq = vecs[v].LengthSquared();
}
}
double size = Math.Sqrt(distSq);
offset = Location.Zero;
mesh = new BEPUphysics.Entities.Prefabs.Sphere(
new BEPUphysics.EntityStateManagement.MotionState()
{
Position = Position.ToBVector(), Orientation = Angle
}, (float)size);
}
offsetmat = Matrix4.CreateTranslation(offset.ToOVector());
}
mesh.Position = Position.ToBVector();
mesh.Orientation = Angle;
}
/// <summary>
/// [Utility] Recovers the object from entity.
/// </summary>
/// <param name="entity">The entity.</param>
/// <returns></returns>
public static IObject RecoverObjectFromEntity(BEPUphysics.Entities.Entity entity)
{
IPhysicObject phy = (entity.CollisionInformation.Tag as IPhysicObject);
if (phy != null)
{
return(phy.ObjectOwner);
}
return(null);
}
private static Vector3 GetAnchorGuess(Entity connectionA, Entity connectionB)
{
var anchor = new Vector3();
if (connectionA != null)
anchor += connectionA.position;
if (connectionB != null)
anchor += connectionB.position;
if (connectionA != null && connectionB != null)
anchor *= 0.5f;
return anchor;
}
/// <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);
}
private static Vector3 GetAnchorGuess(Entity connectionA, Entity connectionB)
{
var anchor = new Vector3();
if (connectionA != null)
anchor.Add( ref connectionA.position, out anchor );
if (connectionB != null)
anchor.Add( ref connectionB.position, out anchor );
if (connectionA != null && connectionB != null)
anchor.Mult( 0.5f, out anchor );
return anchor;
}
/// <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 );
}
private static void GetAnchorGuess( Entity connectionA, Entity connectionB, out Vector3 anchor )
{
if( connectionA != null && connectionB != null )
{
connectionA.position.Add( ref connectionA.position, out anchor );
anchor.Mult( 0.5f, out anchor );
}
else if( connectionA != null )
anchor = connectionA.position;
else if( connectionB != null )
anchor = connectionB.position;
anchor = Vector3.Zero;
}
/// <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>
/// Creates a new EntityModel.
/// </summary>
/// <param name="entity">Entity to attach the graphical representation to.</param>
/// <param name="model">Graphical representation to use for the entity.</param>
/// <param name="transform">Base transformation to apply to the model before moving to the entity.</param>
/// <param name="game">Game to which this component will belong.</param>
public EntityModel(Entity entity, Model model, Matrix transform, Game game, Player player)
: base(game)
{
this.entity = entity;
this.model = model;
Transform = transform;
Player = player;
//Collect any bone transformations in the model itself.
//The default cube model doesn't have any, but this allows the EntityModel to work with more complicated shapes.
boneTransforms = new Matrix[model.Bones.Count];
foreach (var effect in model.Meshes.SelectMany(mesh => mesh.Effects).Cast<BasicEffect>())
{
effect.EnableDefaultLighting();
}
}
/// <summary>
/// Calculates the impulse to apply to the center of mass of physically simulated bodies within the field.
/// </summary>
/// <param name="e">Target of the impulse.</param>
/// <param name="dt">Time since the last frame in simulation seconds.</param>
/// <param name="impulse">Force to apply at the given position.</param>
protected override void CalculateImpulse(Entity e, float dt, out Vector3 impulse)
{
if (MaximumPushSpeed > 0)
{
//Current velocity along the tangent direction.
float dot = Vector3.Dot(e.LinearVelocity, forceDirection);
//Compute the velocity difference between the current and the maximum
dot = MaximumPushSpeed - dot;
//Compute the force needed to reach the maximum, but clamp it to the amount of force that the field can apply
//Also, don't apply a force that would slow an object down.
dot = MathHelper.Clamp(dot * e.Mass, 0, forceMagnitude * dt);
Vector3.Multiply(ref forceDirection, dot, out impulse);
}
else
impulse = Force * dt;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public DetectorVolumeDemo(DemosGame game)
: base(game)
{
var model = game.Content.Load<Model>("tube");
Vector3[] modelVertices;
int[] modelIndices;
ModelDataExtractor.GetVerticesAndIndicesFromModel(model, out modelVertices, out modelIndices);
detectorVolume = new DetectorVolume(new TriangleMesh(new StaticMeshData(modelVertices, modelIndices)));
Space.Add(detectorVolume);
game.ModelDrawer.Add(detectorVolume.TriangleMesh);
//The detector volume works on all of the entity types:
//Convexes!
testEntity = new Box(new Vector3(0, -10, 0), 1, 1, 1);
//Compounds!
//var bodies = new List<CompoundShapeEntry>
//{
// new CompoundShapeEntry(new SphereShape(.5f), new Vector3(0, -8.2f, 0), 1),
// new CompoundShapeEntry(new SphereShape(.5f), new Vector3(0, -9f, 0), 1),
// new CompoundShapeEntry(new SphereShape(.5f), new Vector3(0, -9.8f, 0), 1)
//};
//testEntity = new CompoundBody(bodies);
//Mobile meshes!
//model = game.Content.Load<Model>("tube");
//TriangleMesh.GetVerticesAndIndicesFromModel(model, out modelVertices, out modelIndices);
//testEntity = new MobileMesh(modelVertices, modelIndices, new AffineTransform(new Vector3(.2f, .2f, .2f), Quaternion.Identity, new Vector3(0, -10, 0)), MobileMeshSolidity.Solid);
testEntity.Tag = "noDisplayObject";
displayBox = game.ModelDrawer.Add(testEntity);
SetColor(0);
game.ModelDrawer.IsWireframe = true;
testEntity.LinearVelocity = new Vector3(0, 1, 0);
Space.Add(testEntity);
//The color of the entity will change based upon events.
//The events don't pay attention to the causing events, so you can toss a ball through the volume to recolor the entity.
detectorVolume.EntityBeganTouching += BeganTouching;
detectorVolume.EntityStoppedTouching += StoppedTouching;
detectorVolume.VolumeBeganContainingEntity += BeganContaining;
detectorVolume.VolumeStoppedContainingEntity += StoppedContaining;
game.Camera.Position = new Vector3(0, 0, 22);
Space.ForceUpdater.Gravity = new Vector3();
}
/// <summary>
/// Constructs a new constraint which restricts two 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="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
/// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move and rotate.</param>
/// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
public LineSliderJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 lineAnchor, System.Numerics.Vector3 lineDirection, System.Numerics.Vector3 pointAnchor)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, lineDirection);
Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
Limit.IsActive = false;
Motor.IsActive = false;
Add(PointOnLineJoint);
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 two degrees of linear freedom and all three degrees of angular freedom.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
/// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move.</param>
/// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
public PrismaticJoint(Entity connectionA, Entity connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 pointAnchor)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
AngularJoint = new NoRotationJoint(connectionA, connectionB);
Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
Limit.IsActive = false;
Motor.IsActive = false;
Add(PointOnLineJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Creates a new EntityModel.
/// </summary>
/// <param name="entity">Entity to attach the graphical representation to.</param>
/// <param name="model">Graphical representation to use for the entity.</param>
/// <param name="transform">Base transformation to apply to the model before moving to the entity.</param>
/// <param name="game">Game to which this component will belong.</param>
public EntityModel(Entity entity, Model model, Matrix transform, Game game, Effect effect)
: base(game)
{
this.entity = entity;
this.model = model;
this.Transform = transform;
//Go through each mesh and assign pre-made effects
for (int i = 0; i < model.Meshes.Count; i++)
{
this.effect.Add(model.Meshes[i].Effects[0]);
}
//Collect any bone transformations in the model itself.
//The default cube model doesn't have any, but this allows the EntityModel to work with more complicated shapes.
boneTransforms = new Matrix[model.Bones.Count];
}
/// <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);
}
/// <summary>
/// Creates a new EntityModel.
/// </summary>
/// <param name="entity">Entity to attach the graphical representation to.</param>
/// <param name="model">Graphical representation to use for the entity.</param>
/// <param name="transform">Base transformation to apply to the model before moving to the entity.</param>
/// <param name="game">Game to which this component will belong.</param>
public EntityModel(Entity entity, Model model, Matrix transform, Game game)
: base(game)
{
this.entity = entity;
this.model = model;
this.Transform = transform;
//Collect any bone transformations in the model itself.
//The default cube model doesn't have any, but this allows the EntityModel to work with more complicated shapes.
boneTransforms = new Matrix[model.Bones.Count];
foreach (ModelMesh mesh in model.Meshes)
{
foreach (BasicEffect effect in mesh.Effects)
{
effect.EnableDefaultLighting();
}
}
}
/// <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 constraint which restricts one linear degree of 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="planeAnchor">Location of the anchor for the plane to be attached to connectionA in world space.</param>
/// <param name="planeNormal">Normal of the plane constraint in world space.</param>
/// <param name="xAxis">Direction in world space along which the X axis LinearAxisLimit and LinearAxisMotor work.
/// This is usually chosen to be perpendicular to the planeNormal and the yAxis.</param>
/// <param name="yAxis">Direction in world space along which the Y axis LinearAxisLimit and LinearAxisMotor work.
/// This is usually chosen to be perpendicular to the planeNormal and the xAxis.</param>
/// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
public PlaneSliderJoint(Entity connectionA, Entity connectionB, Vector3 planeAnchor, Vector3 planeNormal, Vector3 xAxis, Vector3 yAxis, Vector3 pointAnchor)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
PointOnPlaneJoint = new PointOnPlaneJoint(connectionA, connectionB, planeAnchor, planeNormal, pointAnchor);
LimitX = new LinearAxisLimit(connectionA, connectionB, planeAnchor, pointAnchor, xAxis, 0, 0);
MotorX = new LinearAxisMotor(connectionA, connectionB, planeAnchor, pointAnchor, xAxis);
LimitY = new LinearAxisLimit(connectionA, connectionB, planeAnchor, pointAnchor, yAxis, 0, 0);
MotorY = new LinearAxisMotor(connectionA, connectionB, planeAnchor, pointAnchor, yAxis);
LimitX.IsActive = false;
MotorX.IsActive = false;
LimitY.IsActive = false;
MotorY.IsActive = false;
Add(PointOnPlaneJoint);
Add(LimitX);
Add(MotorX);
Add(LimitY);
Add(MotorY);
}
/// <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);
}
internal void PreStep(float dt)
{
parentA = contact.collisionPair.ParentA;
parentB = contact.collisionPair.ParentB;
//Set up the jacobians.
linearAX = contact.Normal.X;
linearAY = contact.Normal.Y;
linearAZ = contact.Normal.Z;
//linearBX = -linearAX;
//linearBY = -linearAY;
//linearBZ = -linearAZ;
//angular A = Ra x N
angularAX = (contact.Ra.Y * linearAZ) - (contact.Ra.Z * linearAY);
angularAY = (contact.Ra.Z * linearAX) - (contact.Ra.X * linearAZ);
angularAZ = (contact.Ra.X * linearAY) - (contact.Ra.Y * linearAX);
//Angular B = N x Rb
angularBX = (linearAY * contact.Rb.Z) - (linearAZ * contact.Rb.Y);
angularBY = (linearAZ * contact.Rb.X) - (linearAX * contact.Rb.Z);
angularBZ = (linearAX * contact.Rb.Y) - (linearAY * contact.Rb.X);
//Compute inverse effective mass matrix
float entryA, entryB;
//these are the transformed coordinates
float tX, tY, tZ;
if (parentA.isDynamic)
{
tX = angularAX * parentA.inertiaTensorInverse.M11 + angularAY * parentA.inertiaTensorInverse.M21 + angularAZ * parentA.inertiaTensorInverse.M31;
tY = angularAX * parentA.inertiaTensorInverse.M12 + angularAY * parentA.inertiaTensorInverse.M22 + angularAZ * parentA.inertiaTensorInverse.M32;
tZ = angularAX * parentA.inertiaTensorInverse.M13 + angularAY * parentA.inertiaTensorInverse.M23 + angularAZ * parentA.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + 1 / parentA.mass;
}
else
entryA = 0;
if (parentB.isDynamic)
{
tX = angularBX * parentB.inertiaTensorInverse.M11 + angularBY * parentB.inertiaTensorInverse.M21 + angularBZ * parentB.inertiaTensorInverse.M31;
tY = angularBX * parentB.inertiaTensorInverse.M12 + angularBY * parentB.inertiaTensorInverse.M22 + angularBZ * parentB.inertiaTensorInverse.M32;
tZ = angularBX * parentB.inertiaTensorInverse.M13 + angularBY * parentB.inertiaTensorInverse.M23 + angularBZ * parentB.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + 1 / parentB.mass;
}
else
entryB = 0;
velocityToImpulse = -1 / (entryA + entryB); //Softness?
//Bounciness
bias =
MathHelper.Min(
MathHelper.Max(0, contact.PenetrationDepth - contact.collisionPair.allowedPenetration) *
contact.collisionPair.space.simulationSettings.CollisionResponse.PenetrationRecoveryStiffness / dt,
contact.collisionPair.space.simulationSettings.CollisionResponse.MaximumPositionCorrectionSpeed);
if (contact.collisionPair.Bounciness > 0)
{
//Compute relative velocity
float relativeVelocity = parentA.linearVelocity.X * linearAX + parentA.linearVelocity.Y * linearAY + parentA.linearVelocity.Z * linearAZ +
parentA.angularVelocity.X * angularAX + parentA.angularVelocity.Y * angularAY + parentA.angularVelocity.Z * angularAZ -
parentB.linearVelocity.X * linearAX - parentB.linearVelocity.Y * linearAY - parentB.linearVelocity.Z * linearAZ +
parentB.angularVelocity.X * angularBX + parentB.angularVelocity.Y * angularBY + parentB.angularVelocity.Z * angularBZ;
relativeVelocity *= -1;
if (relativeVelocity > contact.collisionPair.space.simulationSettings.CollisionResponse.BouncinessVelocityThreshold)
bias = MathHelper.Max(relativeVelocity * contact.collisionPair.Bounciness, bias);
}
//Warm starting
#if !WINDOWS
Vector3 linear = new Vector3();
Vector3 angular = new Vector3();
#else
Vector3 linear, angular;
#endif
linear.X = accumulatedImpulse * linearAX;
linear.Y = accumulatedImpulse * linearAY;
linear.Z = accumulatedImpulse * linearAZ;
if (parentA.isDynamic)
{
angular.X = accumulatedImpulse * angularAX;
angular.Y = accumulatedImpulse * angularAY;
angular.Z = accumulatedImpulse * angularAZ;
//parentA.ApplyLinearImpulse(ref linear);
//parentA.ApplyAngularImpulse(ref angular);
}
if (parentB.isDynamic)
{
linear.X = -linear.X;
linear.Y = -linear.Y;
linear.Z = -linear.Z;
angular.X = accumulatedImpulse * angularBX;
angular.Y = accumulatedImpulse * angularBY;
angular.Z = accumulatedImpulse * angularBZ;
//parentB.ApplyLinearImpulse(ref linear);
//parentB.ApplyAngularImpulse(ref angular);
}
}
///<summary>
/// Enqueues a change to an entity's linear velocity.
///</summary>
///<param name="entity">Entity to target.</param>
///<param name="newLinearVelocity">New linear velocity of the entity.</param>
public void EnqueueLinearVelocity(Entity entity, ref System.Numerics.Vector3 newLinearVelocity)
{
stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newLinearVelocity, targetField = TargetField.LinearVelocity });
}
///<summary>
/// Enqueues a change to an entity's position.
///</summary>
///<param name="entity">Entity to target.</param>
///<param name="newPosition">New position of the entity.</param>
public void EnqueuePosition(Entity entity, ref System.Numerics.Vector3 newPosition)
{
stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newPosition, targetField = TargetField.Position });
}
/// <summary>
/// Locates the closest support entity by performing a raycast at collected candidates.
/// </summary>
/// <param name="supportEntity">The closest supporting entity.</param>
/// <param name="supportLocation">The support location where the ray hit the entity.</param>
/// <param name="supportNormal">The normal at the surface where the ray hit the entity.</param>
/// <param name="supportDistance">Distance from the character to the support location.</param>
/// <returns>Whether or not a support was located.</returns>
private bool findSupport(out object supportEntityTag, out BEPUphysics.Entities.Entity supportEntity, out Vector3 supportLocation, out Vector3 supportNormal, out float supportDistance)
{
supportEntity = null;
supportEntityTag = null;
supportLocation = BEPUutilities.Toolbox.NoVector;
supportNormal = BEPUutilities.Toolbox.NoVector;
supportDistance = float.MaxValue;
const float fudgeFactor = 0.1f;
Vector3 rayOrigin = this.Body.Position;
rayOrigin.Y += fudgeFactor + this.Height * -0.5f;
for (int i = 0; i < this.collisionPairCollector.CollisionInformation.Pairs.Count; i++)
{
var pair = this.collisionPairCollector.CollisionInformation.Pairs[i];
//Determine which member of the collision pair is the possible support.
Collidable candidate = (pair.BroadPhaseOverlap.EntryA == collisionPairCollector.CollisionInformation ? pair.BroadPhaseOverlap.EntryB : pair.BroadPhaseOverlap.EntryA) as Collidable;
//Ensure that the candidate is a valid supporting entity.
if (candidate.CollisionRules.Personal >= CollisionRule.NoSolver)
{
continue; //It is invalid!
}
if (candidate.CollisionRules.Group == Character.NoCollideGroup)
{
continue;
}
//The maximum length is supportHeight * 2 instead of supportHeight alone because the character should be able to step downwards.
//This acts like a sort of 'glue' to help the character stick on the ground in general.
float maximumDistance;
//The 'glue' effect should only occur if the character has a solid hold on the ground though.
//Otherwise, the character is falling or sliding around uncontrollably.
if (this.HasTraction && !this.IsSwimming)
{
maximumDistance = fudgeFactor + (this.SupportHeight * 2.0f);
}
else
{
maximumDistance = fudgeFactor + this.SupportHeight;
}
foreach (Vector3 rayStart in this.rayOffsets.Select(x => x + rayOrigin))
{
BEPUutilities.RayHit rayHit;
// Fire a ray at the candidate and determine some details!
if (candidate.RayCast(new Ray(rayStart, Vector3.Down), maximumDistance, out rayHit))
{
Vector3 n = Vector3.Normalize(rayHit.Normal);
if (n.Y > supportNormal.Y)
{
supportNormal = n;
}
// We want to find the closest support, so compare it against the last closest support.
if (rayHit.T < supportDistance && n.Y > 0.25f)
{
supportDistance = rayHit.T - fudgeFactor;
supportLocation = rayHit.Location;
if (rayHit.T < 0.0f)
{
supportNormal = Vector3.Up;
}
var entityInfo = candidate as EntityCollidable;
if (entityInfo != null)
{
supportEntity = entityInfo.Entity;
supportEntityTag = supportEntity != null ? supportEntity.Tag : candidate.Tag;
}
else
{
supportEntityTag = candidate.Tag;
}
}
}
}
}
bool isSupported = supportDistance < float.MaxValue;
return(isSupported);
}
/// <summary>
/// Determines if the entity is affected by the force field.
/// </summary>
/// <param name="testEntity">Entity to test.</param>
/// <returns>Whether the entity is affected.</returns>
public override bool IsEntityAffected(Entity testEntity)
{
return true;
}
/// <summary>
/// [Utility] Recovers the physic object from entity.
/// </summary>
/// <param name="entity">The entity.</param>
/// <returns></returns>
public static IPhysicObject RecoverIPhysicObjectFromEntity(BEPUphysics.Entities.Entity entity)
{
return(entity.CollisionInformation.Tag as IPhysicObject);
}
