/// <summary>
/// Computes the intersection, if any, between a ray and the objects in the character's bounding box.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="length">Length of the ray to use in units of the ray's length.</param>
/// <param name="earliestHit">Earliest intersection location and information.</param>
/// <param name="hitObject">Collidable intersected by the ray, if any.</param>
/// <returns>Whether or not the ray hit anything.</returns>
public bool RayCast(Ray ray, float length, out RayHit earliestHit, out Collidable hitObject)
{
earliestHit = new RayHit();
earliestHit.T = float.MaxValue;
hitObject = null;
foreach (var collidable in characterBody.CollisionInformation.OverlappedCollidables)
{
//Check to see if the collidable is hit by the ray.
float t;
if (ray.Intersects(ref collidable.boundingBox, out t) && t < length)
{
//Is it an earlier hit than the current earliest?
RayHit hit;
if (collidable.RayCast(ray, length, SupportRayFilter, out hit) && hit.T < earliestHit.T)
{
earliestHit = hit;
hitObject = collidable;
}
}
}
if (earliestHit.T == float.MaxValue)
return false;
return true;
}
private void CollisionDetectedHandler(EntityCollidable sender, Collidable other, CollidablePairHandler pair)
{
EntityCollidable otherEntityCollidable = other as EntityCollidable;
Terrain otherTerrain = other as Terrain;
if (otherEntityCollidable != null &&
otherEntityCollidable.Entity != null &&
otherEntityCollidable.Entity.Tag != null)
{
int actorId = (int)(otherEntityCollidable.Entity.Tag);
if (actorId == mOwnerActorId)
return;
Actor actorHit = GameResources.ActorManager.GetActorById(actorId);
IDamagable damage = actorHit.GetBehaviorThatImplementsType<IDamagable>();
if (damage != null)
{
damage.TakeDamage(mDamage);
}
Impact();
}
else if (otherTerrain != null)
{
Impact();
}
}
/// <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;
}
void Events_InitialCollisionDetected(EntityCollidable sender, Collidable other, CollidablePairHandler pair)
{
if ("Environment".Equals(other.Tag))
{
// do something on collision
}
}
public void InitialCollisionDetected(EntityCollidable sender, Collidable other, CollidablePairHandler collisionPair)
{
if (other == acceptedTrigger)
{
//If the detector collided with the accepted trigger, move the box.
movedBox.Position = new Vector3(4, 5, 0);
movedBox.Orientation = Quaternion.Identity;
movedBox.LinearVelocity = Vector3.Zero;
movedBox.AngularVelocity = Vector3.Zero;
}
}
private void InitialCollisionDetectedHandler(EntityCollidable sender, Collidable other, CollidablePairHandler pair)
{
EntityCollidable otherEntityCollidable = other as EntityCollidable;
if (otherEntityCollidable != null &&
otherEntityCollidable.Entity != null &&
otherEntityCollidable.Entity.Tag != null &&
mTriggerSet &&
GameResources.ActorManager.IsPlayer((int)(otherEntityCollidable.Entity.Tag)))
{
mTriggerSet = false;
GameResources.LoadNewLevelDelegate(mLevelName);
}
}
///<summary>
/// Initializes the manifold.
///</summary>
///<param name="newCollidableA">First collidable.</param>
///<param name="newCollidableB">Second collidable.</param>
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
convex = newCollidableA as ConvexCollidable;
mesh = newCollidableB as MobileMeshCollidable;
if (convex == null || mesh == null)
{
convex = newCollidableB as ConvexCollidable;
mesh = newCollidableA as MobileMeshCollidable;
if (convex == null || mesh == null)
throw new ArgumentException("Inappropriate types used to initialize contact manifold.");
}
}
private void InitialCollisionDetectedHandler(EntityCollidable sender, Collidable other, CollidablePairHandler pair)
{
EntityCollidable otherEntityCollidable = other as EntityCollidable;
if (otherEntityCollidable != null &&
otherEntityCollidable.Entity != null &&
otherEntityCollidable.Entity.Tag != null &&
mTriggerSet &&
GameResources.ActorManager.IsPlayer((int)(otherEntityCollidable.Entity.Tag)))
{
PlayerView finder = GameResources.ActorManager.GetPlayerViewOfAvatar((int)(otherEntityCollidable.Entity.Tag));
finder.AvatarDesc.ObtainItem(Item);
mTriggerSet = false;
Owner.Despawn();
}
}
/// <summary>
/// Raises the appropriate ParentObject collision events depending on its CollisionType value
/// </summary>
/// <param name="sender">The current ISpaceObject instance</param>
/// <param name="other">The ISpaceObject instance which collided</param>
/// <param name="pair"/>
protected void OnCollisionDetected(Collidable sender, Collidable other, CollidablePairHandler pair)
{
if (sender.Tag == null || other.Tag == null)
return;
ICollisionObject senderCollisionObject = (ICollisionObject) sender.Tag;
ICollisionObject otherCollisionObject = (ICollisionObject) other.Tag;
switch (ParentObject.CollisionType)
{
case CollisionType.Trigger:
{
ParentObject.OnCollisionTrigger(senderCollisionObject, otherCollisionObject);
break;
}
case CollisionType.Collide:
{
var collisionPoint = new CollisionPoint
{
ContactObject = otherCollisionObject,
ContactPoint = pair.Contacts[0].Contact.Position,
ContactTime = pair.TimeOfImpact,
Material = null,
SurfaceNormal = pair.Contacts[0].Contact.Normal
};
ParentObject.OnCollisionReact(senderCollisionObject, otherCollisionObject, collisionPoint, ref _collisionHandled);
break;
}
default:
case CollisionType.None:
{
break;
}
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us.
if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
System.Numerics.Vector3 velocity;
Vector3Ex.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
var triangle = PhysicsThreadResources.GetTriangle();
triangle.collisionMargin = 0;
System.Numerics.Vector3 terrainUp = new System.Numerics.Vector3(terrain.worldTransform.LinearTransform.M21, terrain.worldTransform.LinearTransform.M22, terrain.worldTransform.LinearTransform.M23);
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < TerrainManifold.overlappedTriangles.Count; i++)
{
terrain.Shape.GetTriangle(TerrainManifold.overlappedTriangles.Elements[i], ref terrain.worldTransform, out triangle.vA, out triangle.vB, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3Ex.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3Ex.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3Ex.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
System.Numerics.Vector3 AB, AC;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out AC);
System.Numerics.Vector3 normal;
Vector3Ex.Cross(ref AC, ref AB, out normal);
float dot;
Vector3Ex.Dot(ref normal, ref terrainUp, out dot);
if (dot < 0)
Vector3Ex.Dot(ref normal, ref rayHit.Normal, out dot);
else
{
Vector3Ex.Dot(ref normal, ref rayHit.Normal, out dot);
dot = -dot;
}
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (dot < 0)
{
timeOfImpact = rayHit.T;
}
}
}
PhysicsThreadResources.GiveBack(triangle);
}
}
}
///<summary>
/// Initializes the manifold.
///</summary>
///<param name="newCollidableA">First collidable.</param>
///<param name="newCollidableB">Second collidable.</param>
///<exception cref="Exception">Thrown when the collidables being used are not of the proper type.</exception>
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
box = newCollidableA as ConvexCollidable<BoxShape>;
sphere = newCollidableB as ConvexCollidable<SphereShape>;
if (box == null || sphere == null)
{
box = newCollidableB as ConvexCollidable<BoxShape>;
sphere = newCollidableA as ConvexCollidable<SphereShape>;
if (box == null || sphere == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair.");
}
}
}
///<summary>
/// Initializes the manifold.
///</summary>
///<param name="newCollidableA">First collidable.</param>
///<param name="newCollidableB">Second collidable.</param>
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
collidableA = newCollidableA as ConvexCollidable;
collidableB = newCollidableB as ConvexCollidable;
pairTester.Initialize(newCollidableA, newCollidableB);
if (collidableA == null || collidableB == null)
{
throw new Exception("Inappropriate types used to initialize pair tester.");
}
}
/// <summary>
/// Removes a collidable from the group.
/// </summary>
/// <param name="collidable">Collidable to remove.</param>
public void Remove(Collidable collidable)
{
CollidableTree.Remove(collidable);
}
///<summary> /// Initializes the manifold. ///</summary> ///<param name="newCollidableA">First collidable.</param> ///<param name="newCollidableB">Second collidable.</param> public abstract void Initialize(Collidable newCollidableA, Collidable newCollidableB);
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
var collidableA = CollidableA as ConvexCollidable;
var collidableB = CollidableB as ConvexCollidable;
var modeA = collidableA.entity == null ? PositionUpdateMode.Discrete : collidableA.entity.PositionUpdateMode;
var modeB = collidableB.entity == null ? PositionUpdateMode.Discrete : collidableB.entity.PositionUpdateMode;
var overlap = BroadPhaseOverlap;
if (
(overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
(
(
modeA == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
modeB == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
modeA == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
modeB == PositionUpdateMode.Continuous
)
)
)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
//Discrete objects have already had their linear motion integrated, so don't use their velocity.
Vector3 velocity;
if (modeA == PositionUpdateMode.Discrete)
{
//CollidableA is static for the purposes of this continuous test.
velocity = collidableB.entity.linearVelocity;
}
else if (modeB == PositionUpdateMode.Discrete)
{
//CollidableB is static for the purposes of this continuous test.
Vector3.Negate(ref collidableA.entity.linearVelocity, out velocity);
}
else
{
//Both objects are moving.
Vector3.Subtract(ref collidableB.entity.linearVelocity, ref collidableA.entity.linearVelocity, out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadiusA = collidableA.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadiusA * minimumRadiusA < velocitySquared)
{
//Spherecast A against B.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(collidableA.worldTransform.Position, -velocity), minimumRadiusA, collidableB.Shape, ref collidableB.worldTransform, timeOfImpact, out rayHit))
timeOfImpact = rayHit.T;
}
var minimumRadiusB = collidableB.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
if (minimumRadiusB * minimumRadiusB < velocitySquared)
{
//Spherecast B against A.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(collidableB.worldTransform.Position, velocity), minimumRadiusB, collidableA.Shape, ref collidableA.worldTransform, timeOfImpact, out rayHit))
timeOfImpact = rayHit.T;
}
//If it's intersecting, throw our hands into the air and give up.
//This is generally a perfectly acceptable thing to do, since it's either sitting
//inside another object (no ccd makes sense) or we're still in an intersecting case
//from a previous frame where CCD took place and a contact should have been created
//to deal with interpenetrating velocity. Sometimes that contact isn't sufficient,
//but it's good enough.
if (timeOfImpact == 0)
timeOfImpact = 1;
}
}
/// <summary>
/// Finds a supporting entity, the contact location, and the contact normal.
/// </summary>
/// <param name="location">Contact point between the wheel and the support.</param>
/// <param name="normal">Contact normal between the wheel and the support.</param>
/// <param name="suspensionLength">Length of the suspension at the contact.</param>
/// <param name="supportingCollidable">Collidable supporting the wheel, if any.</param>
/// <param name="entity">Supporting object.</param>
/// <param name="material">Material of the wheel.</param>
/// <returns>Whether or not any support was found.</returns>
protected internal override bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material)
{
suspensionLength = float.MaxValue;
location = Toolbox.NoVector;
supportingCollidable = null;
entity = null;
normal = Toolbox.NoVector;
material = null;
Collidable testCollidable;
RayHit rayHit;
bool hit = false;
for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++)
{
var pair = detector.CollisionInformation.pairs[i];
testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable;
if (testCollidable != null)
{
if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal &&
testCollidable.RayCast(new Ray(wheel.suspension.worldAttachmentPoint, wheel.suspension.worldDirection), wheel.suspension.restLength, out rayHit) &&
rayHit.T < suspensionLength)
{
suspensionLength = rayHit.T;
EntityCollidable entityCollidable;
if ((entityCollidable = testCollidable as EntityCollidable) != null)
{
entity = entityCollidable.Entity;
material = entityCollidable.Entity.Material;
}
else
{
entity = null;
supportingCollidable = testCollidable;
var materialOwner = testCollidable as IMaterialOwner;
if (materialOwner != null)
material = materialOwner.Material;
}
location = rayHit.Location;
normal = rayHit.Normal;
hit = true;
}
}
}
if (hit)
{
if (suspensionLength > 0)
normal.Normalize();
else
Vector3.Negate(ref wheel.suspension.worldDirection, out normal);
return true;
}
return false;
}
///<summary>
/// Initializes the manifold.
///</summary>
///<param name="newCollidableA">First collidable.</param>
///<param name="newCollidableB">Second collidable.</param>
///<exception cref="Exception">Thrown when the collidables being used are not of the proper type.</exception>
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
boxA = (ConvexCollidable<BoxShape>)newCollidableA;
boxB = (ConvexCollidable<BoxShape>)newCollidableB;
if (boxA == null || boxB == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair tester.");
}
}
public override void UpdateTimeOfImpact( Collidable requester, float dt )
{
//Complicated and not vital. Leaving it out for simplicity. Check out InstancedMeshPairHandler for an example implementation.
//Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us.
if( convex.IsActive && convex.Entity.PositionUpdateMode == PositionUpdateMode.Continuous )
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity = convex.Entity.LinearVelocity * dt;
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if( minimumRadius * minimumRadius < velocitySquared )
{
for( int i = 0; i < contactManifold.ActivePairs.Count; ++i )
{
var pair = contactManifold.ActivePairs.Values[i];
//In the contact manifold, the box collidable is always put into the second slot.
var boxCollidable = (ReusableBoxCollidable)pair.CollidableB;
RayHit rayHit;
var worldTransform = boxCollidable.WorldTransform;
if( GJKToolbox.CCDSphereCast( new Ray( convex.WorldTransform.Position, velocity ), minimumRadius, boxCollidable.Shape, ref worldTransform, timeOfImpact, out rayHit ) &&
rayHit.T > Toolbox.BigEpsilon )
{
timeOfImpact = rayHit.T;
}
}
}
}
}
public override void Initialize( Collidable newCollidableA, Collidable newCollidableB )
{
convex = newCollidableA as ConvexCollidable;
voxelBlob = newCollidableB as VoxelBlob;
if( convex == null || voxelBlob == null )
{
convex = newCollidableB as ConvexCollidable;
voxelBlob = newCollidableA as VoxelBlob;
if( convex == null || voxelBlob == null )
throw new ArgumentException( "Inappropriate types used to initialize contact manifold." );
}
ActivePairs = new QuickDictionary<Int3, GeneralConvexPairTester>( BufferPools<Int3>.Locking, BufferPools<GeneralConvexPairTester>.Locking, BufferPools<int>.Locking, 3 );
activePairsBackBuffer = new QuickDictionary<Int3, GeneralConvexPairTester>( BufferPools<Int3>.Locking, BufferPools<GeneralConvexPairTester>.Locking, BufferPools<int>.Locking, 3 );
}
void Events_ContactCreated(EntityCollidable sender, Collidable other, BEPUphysics.NarrowPhaseSystems.Pairs.CollidablePairHandler pair, ContactData contact)
{
this.Collided.Execute(other, pair.Contacts);
}
///<summary>
/// Initializes the manifold.
///</summary>
///<param name="newCollidableA">First collidable.</param>
///<param name="newCollidableB">Second collidable.</param>
///<exception cref="Exception">Thrown when the collidables being used are not of the proper type.</exception>
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
sphereA = (ConvexCollidable<SphereShape>)newCollidableA;
sphereB = (ConvexCollidable<SphereShape>)newCollidableB;
if (sphereA == null || sphereB == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair.");
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
var overlap = BroadPhaseOverlap;
var triangleMode = triangle.entity == null ? PositionUpdateMode.Discrete : triangle.entity.PositionUpdateMode;
var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
(
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
triangleMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
triangleMode == PositionUpdateMode.Continuous
)
)
)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convexMode == PositionUpdateMode.Discrete)
{
//Triangle is static for the purposes of this continuous test.
velocity = triangle.entity.linearVelocity;
}
else if (triangleMode == PositionUpdateMode.Discrete)
{
//Convex is static for the purposes of this continuous test.
Vector3.Negate(ref convex.entity.linearVelocity, out velocity);
}
else
{
//Both objects are moving.
Vector3.Subtract(ref triangle.entity.linearVelocity, ref convex.entity.linearVelocity, out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadiusA = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadiusA * minimumRadiusA < velocitySquared)
{
//Spherecast A against B.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(convex.worldTransform.Position, -velocity), minimumRadiusA, triangle.Shape, ref triangle.worldTransform, timeOfImpact, out rayHit))
{
if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
{
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
Vector3 AB, AC;
Vector3.Subtract(ref triangle.Shape.vB, ref triangle.Shape.vA, out AB);
Vector3.Subtract(ref triangle.Shape.vC, ref triangle.Shape.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
if (triangle.Shape.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
triangle.Shape.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
//TECHNICALLY, the triangle should be casted too. But, given the way triangles are usually used and their tiny minimum radius, ignoring it is usually just fine.
//var minimumRadiusB = triangle.minimumRadius * MotionSettings.CoreShapeScaling;
//if (minimumRadiusB * minimumRadiusB < velocitySquared)
//{
// //Spherecast B against A.
// RayHit rayHit;
// if (GJKToolbox.SphereCast(new Ray(triangle.entity.position, velocity), minimumRadiusB, convex.Shape, ref convex.worldTransform, 1, out rayHit) &&
// rayHit.T < timeOfImpact)
// {
// if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
// {
// float dot;
// Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
// if (dot > 0)
// {
// timeOfImpact = rayHit.T;
// }
// }
// else
// {
// timeOfImpact = rayHit.T;
// }
// }
//}
//If it's intersecting, throw our hands into the air and give up.
//This is generally a perfectly acceptable thing to do, since it's either sitting
//inside another object (no ccd makes sense) or we're still in an intersecting case
//from a previous frame where CCD took place and a contact should have been created
//to deal with interpenetrating velocity. Sometimes that contact isn't sufficient,
//but it's good enough.
if (timeOfImpact == 0)
timeOfImpact = 1;
}
}
static bool DefaultCCDFilter(Entity entity, Collidable other, CollidablePairHandler pair)
{
return pair.broadPhaseOverlap.collisionRule < CollisionRule.NoSolver;
}
/// <summary>
/// Finds a supporting entity, the contact location, and the contact normal.
/// </summary>
/// <param name="location">Contact point between the wheel and the support.</param>
/// <param name="normal">Contact normal between the wheel and the support.</param>
/// <param name="suspensionLength">Length of the suspension at the contact.</param>
/// <param name="supportingCollidable">Collidable supporting the wheel, if any.</param>
/// <param name="entity">Supporting object.</param>
/// <param name="material">Material of the wheel.</param>
/// <returns>Whether or not any support was found.</returns>
protected internal override bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material)
{
suspensionLength = float.MaxValue;
location = Toolbox.NoVector;
supportingCollidable = null;
entity = null;
normal = Toolbox.NoVector;
material = null;
Collidable testCollidable;
RayHit rayHit;
bool hit = false;
Quaternion localSteeringTransform;
Quaternion.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform);
var startingTransform = new RigidTransform
{
Position = wheel.suspension.worldAttachmentPoint,
Orientation = Quaternion.Concatenate(Quaternion.Concatenate(LocalWheelOrientation, IncludeSteeringTransformInCast ? localSteeringTransform : Quaternion.Identity), wheel.vehicle.Body.orientation)
};
Vector3 sweep;
Vector3.Multiply(ref wheel.suspension.worldDirection, wheel.suspension.restLength, out sweep);
for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++)
{
var pair = detector.CollisionInformation.pairs[i];
testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable;
if (testCollidable != null)
{
if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal &&
testCollidable.ConvexCast(shape, ref startingTransform, ref sweep, out rayHit) &&
rayHit.T * wheel.suspension.restLength < suspensionLength)
{
suspensionLength = rayHit.T * wheel.suspension.restLength;
EntityCollidable entityCollidable;
if ((entityCollidable = testCollidable as EntityCollidable) != null)
{
entity = entityCollidable.Entity;
material = entityCollidable.Entity.Material;
}
else
{
entity = null;
supportingCollidable = testCollidable;
var materialOwner = testCollidable as IMaterialOwner;
if (materialOwner != null)
material = materialOwner.Material;
}
location = rayHit.Location;
normal = rayHit.Normal;
hit = true;
}
}
}
if (hit)
{
if (suspensionLength > 0)
{
float dot;
Vector3.Dot(ref normal, ref wheel.suspension.worldDirection, out dot);
if (dot > 0)
{
//The cylinder cast produced a normal which is opposite of what we expect.
Vector3.Negate(ref normal, out normal);
}
normal.Normalize();
}
else
Vector3.Negate(ref wheel.suspension.worldDirection, out normal);
return true;
}
return false;
}
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
convex = newCollidableA as ConvexCollidable;
mesh = newCollidableB as FullChunkObject;
if (convex == null || mesh == null)
{
convex = newCollidableB as ConvexCollidable;
mesh = newCollidableA as FullChunkObject;
if (convex == null || mesh == null)
{
throw new ArgumentException("Inappropriate types used to initialize contact manifold.");
}
}
ActivePairs = new QuickDictionary<Vector3i, GeneralConvexPairTester>(BufferPools<Vector3i>.Locking, BufferPools<GeneralConvexPairTester>.Locking, BufferPools<int>.Locking, 3);
activePairsBackBuffer = new QuickDictionary<Vector3i, GeneralConvexPairTester>(BufferPools<Vector3i>.Locking, BufferPools<GeneralConvexPairTester>.Locking, BufferPools<int>.Locking, 3);
}
public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
{
convex = newCollidableA as ConvexCollidable;
triangle = newCollidableB as ConvexCollidable<TriangleShape>;
if (convex == null || triangle == null)
{
convex = newCollidableB as ConvexCollidable;
triangle = newCollidableA as ConvexCollidable<TriangleShape>;
if (convex == null || triangle == null)
throw new ArgumentException("Inappropriate types used to initialize contact manifold.");
}
pairTester.Initialize(convex.Shape);
}
public void CollisionEnded(EntityCollidable sender, Collidable other, CollidablePairHandler collisionPair)
{
if (other == acceptedTrigger)
{
//If the detector ceases to collide, get rid of the spawned box.
movedBox.Position = new Vector3(-4, 5, 0);
movedBox.Orientation = Quaternion.Identity;
movedBox.LinearVelocity = Vector3.Zero;
movedBox.AngularVelocity = Vector3.Zero;
}
}
/// <summary>
/// Adds a new collidable to the group.
/// </summary>
/// <param name="collidable">Collidable to remove.</param>
public void Add(Collidable collidable)
{
CollidableTree.Add(collidable);
}
/// <summary> /// Finds a supporting entity, the contact location, and the contact normal. /// </summary> /// <param name="location">Contact point between the wheel and the support.</param> /// <param name="normal">Contact normal between the wheel and the support.</param> /// <param name="suspensionLength">Length of the suspension at the contact.</param> /// <param name="supportCollidable">Collidable supporting the wheel, if any.</param> /// <param name="entity">Entity supporting the wheel, if any.</param> /// <param name="material">Material of the support.</param> /// <returns>Whether or not any support was found.</returns> protected internal abstract bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportCollidable, out Entity entity, out Material material);
float GetSubmergedHeight(Collidable info, float maxLength, float boundingBoxHeight, ref Vector3 rayOrigin, ref Vector3 xSpacing, ref Vector3 zSpacing, int i, int j, out Vector3 volumeCenter)
{
Ray ray;
Vector3.Multiply(ref xSpacing, i, out ray.Position);
Vector3.Multiply(ref zSpacing, j, out ray.Direction);
Vector3.Add(ref ray.Position, ref ray.Direction, out ray.Position);
Vector3.Add(ref ray.Position, ref rayOrigin, out ray.Position);
ray.Direction = upVector;
//do a bottom-up raycast.
BEPUutilities.RayHit rayHit;
//Only go up to maxLength. If it's further away than maxLength, then it's above the water and it doesn't contribute anything.
if (info.RayCast(ray, maxLength, out rayHit))
{
//Position the ray to point from the other side.
Vector3.Multiply(ref ray.Direction, boundingBoxHeight, out ray.Direction);
Vector3.Add(ref ray.Position, ref ray.Direction, out ray.Position);
Vector3.Negate(ref upVector, out ray.Direction);
float bottomY = rayHit.Location.Y;
float bottom = rayHit.T;
Vector3 bottomPosition = rayHit.Location;
if (info.RayCast(ray, boundingBoxHeight - rayHit.T, out rayHit))
{
Vector3.Add(ref rayHit.Location, ref bottomPosition, out volumeCenter);
Vector3.Multiply(ref volumeCenter, .5f, out volumeCenter);
return Math.Min(surfacePlaneHeight - bottomY, boundingBoxHeight - rayHit.T - bottom);
}
//This inner raycast should always hit, but just in case it doesn't due to some numerical problem, give it a graceful way out.
volumeCenter = Vector3.Zero;
return 0;
}
volumeCenter = Vector3.Zero;
return 0;
}
