public KeyControl(float size)
{
array = new ConvexShape(8);
array.SetPoint(0, new Vector2f(0, -size));
array.SetPoint(1, new Vector2f(.3f * size, -.3f * size));
array.SetPoint(2, new Vector2f(size, 0));
array.SetPoint(3, new Vector2f(.3f * size, .3f * size));
array.SetPoint(4, new Vector2f(0, size));
array.SetPoint(5, new Vector2f(-.3f * size, .3f * size));
array.SetPoint(6, new Vector2f(-size, 0));
array.SetPoint(7, new Vector2f(-.3f * size, -.3f * size));
array.OutlineColor = Color.Black;
array.OutlineThickness = -1f;
var tmp = new CustomHitbox();
for (uint i = 0; i < 8; i++)
{
tmp.CustomShape.Add(array.GetPoint(i));
}
HitBox = tmp;
}
/// <summary>
/// Returns information on what a cuboid-shaped line trace would collide with, if anything.
/// </summary>
/// <param name="start">The start of the line.</param>
/// <param name="end">The end of the line.</param>
/// <param name="filter">The collision filter, input a BEPU BroadPhaseEntry and output whether collision should be allowed.</param>
/// <returns>The collision details.</returns>
public CollisionResult CuboidLineTrace(ConvexShape shape, Location start, Location end, Func <BroadPhaseEntry, bool> filter = null)
{
Vector3 e = new Vector3((double)(end.X - start.X), (double)(end.Y - start.Y), (double)(end.Z - start.Z));
RigidTransform rt = new RigidTransform(new Vector3((double)start.X, (double)start.Y, (double)start.Z));
RayCastResult rcr;
bool hit;
if (filter == null)
{
hit = World.ConvexCast(shape, ref rt, ref e, out rcr);
}
else
{
hit = World.ConvexCast(shape, ref rt, ref e, filter, out rcr);
}
CollisionResult cr = new CollisionResult();
cr.Hit = hit;
if (hit)
{
cr.Normal = new Location(rcr.HitData.Normal);
cr.Position = new Location(rcr.HitData.Location);
if (rcr.HitObject is EntityCollidable)
{
cr.HitEnt = ((EntityCollidable)rcr.HitObject).Entity;
}
else
{
cr.HitEnt = null; // Impacted static world
}
}
else
{
cr.Normal = Location.Zero;
cr.Position = end;
cr.HitEnt = null;
}
return(cr);
}
protected override void OnInitializePhysics()
{
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// ground
CollisionShape groundShape = new BoxShape(50, 1, 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
// Objects
//colShape = new BoxShape(1);
Vector3[] points0 =
{
new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)
};
Vector3[] points1 =
{
new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1), new Vector3(0, 0, -1), new Vector3(-1, -1, 0)
};
colShape0 = new ConvexHullShape(points0);
colShape1 = new ConvexHullShape(points1);
CollisionShapes.Add(colShape0);
CollisionShapes.Add(colShape1);
/*body2 =*/ LocalCreateRigidBody(0, body2Position, colShape1);
rotBody = LocalCreateRigidBody(0, rotBodyPosition, colShape0);
rotBody.CollisionFlags |= CollisionFlags.KinematicObject;
rotBody.ActivationState = ActivationState.DisableDeactivation;
}
private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB)
{
var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);
Vector3 closestPoint;
int count = 0;
while (true)
{
if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary.
closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
{
//Intersecting.
localClosestPointA = Toolbox.ZeroVector;
localClosestPointB = Toolbox.ZeroVector;
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(true);
}
if (++count > MaximumGJKIterations)
{
break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron.
}
if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
{
//No progress towards origin, not intersecting.
break;
}
}
//Compute closest points from the contributing simplexes and barycentric coordinates
simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
//simplex.VerifyContributions();
//if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
// Debug.WriteLine("break.");
simplex.UpdateCachedSimplex(ref cachedSimplex);
return(false);
}
/// <summary>
/// Starts the character controller.
/// </summary>
/// <param name="time"></param>
protected override void OnStart(GameTime time)
{
base.OnStart(time);
if (this.Node.Scene.PhysicsEnabled)
{
this.shape = new CapsuleShape(characterWidth, characterHeight);
this.ghostObject = new PairCachingGhostObject();
this.ghostObject.WorldTransform = this.Node.Transform.WorldTransform.ToBullet();
this.ghostObject.CollisionShape = this.shape;
this.ghostObject.CollisionFlags = CollisionFlags.CharacterObject;
this.kcc = new KinematicCharacterController(this.ghostObject, this.shape, stepHeight);
this.kcc.SetUpAxis(1);
this.Node.Scene.World.AddCollisionObject(
this.ghostObject,
CollisionFilterGroups.CharacterFilter,
CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
this.Node.Scene.World.AddAction(this.kcc);
}
}
public static CollisionInfo MultiColCheck(ConvexShape single, List <ConvexShape> multi)
{
CollisionInfo myInfo = new CollisionInfo();
myInfo.isCollide = false;
myInfo.shapes = new List <ConvexShape>();
int intervalAmount = 0;
foreach (ConvexShape s in multi)
{
CollisionInfo myInfo2 = CheckCol(single, s);
if (myInfo2.isCollide)
{
myInfo.minIntervalDist += myInfo2.minIntervalDist;
intervalAmount += 1;
myInfo.isCollide = true;
myInfo.shapes.Add(myInfo2.shapes[0]);
}
}
//if (myInfo.isCollide) { myInfo.minIntervalDist = myInfo.minIntervalDist / intervalAmount; }
return(myInfo);
}
public static void ProjectPolygon(Vector2f axis, ConvexShape polygon,
ref float min, ref float max)
{
// To project a point on an axis use the dot product
float dotProduct = Trig.DotProduct(axis, polygon.Transform.TransformPoint(polygon.GetPoint(0)));
min = dotProduct;
max = dotProduct;
for (uint i = 0; i < polygon.GetPointCount(); i++)
{
dotProduct = Trig.DotProduct(polygon.Transform.TransformPoint(polygon.GetPoint(i)), axis);
if (dotProduct < min)
{
min = dotProduct;
}
else
{
if (dotProduct > max)
{
max = dotProduct;
}
}
}
}
/// <summary>
/// Performs a convex cast to determine if a step of a given height is valid.
/// This means that stepping up onto the new support wouldn't shove the character's head into a ceiling or other obstacle.
/// </summary>
/// <param name="hitDistance">Vertical distance from the convex cast start to the hit location.</param>
/// <returns>Whether or not the step is safe.</returns>
private bool IsStepSafe(float hitDistance)
{
float stepHeight = maximumStepHeight - hitDistance;
var sweep = new Vector3(0, stepHeight + Body.CollisionInformation.Shape.CollisionMargin, 0);
Vector3 startingLocation = headBlockageFinderOffset + Body.Position;
foreach (Entity candidate in headCollisionPairCollector.CollisionInformation.OverlappedEntities)
{
//foreach (CollisionPair pair in headCollisionPairCollector.CollisionPairs)
//{
// //Determine which member of the collision pair is the possible blockage.
// //The comparisons are all kept on a "parent" as opposed to "collider" level so that interaction with compound shapes is simpler.
// Entity candidate = pair.ParentA == headCollisionPairCollector ? pair.ParentB : pair.ParentA;
//Ensure that the candidate is a valid blocking entity.
if (candidate.CollisionInformation.CollisionRules.Personal > CollisionRule.Normal)
{
continue; //It is invalid!
}
//Fire a convex cast at the candidate and determine some details!
ConvexShape targetShape = candidate.CollisionInformation.Shape as ConvexShape;
if (targetShape != null)
{
RigidTransform sweepTransform = new RigidTransform(startingLocation);
RigidTransform targetTransform = new RigidTransform(candidate.Position, candidate.Orientation);
RayHit rayHit;
if (GJKToolbox.ConvexCast(castingShape, targetShape, ref sweep,
ref sweepTransform, ref targetTransform,
out rayHit))
{
return(false);
}
}
}
return(true);
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="shape">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform,
float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePointWithoutMargin(ref v, out p);
Vector3 contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3.Add(ref p, ref contribution, out p);
Vector3.Subtract(ref hit.Location, ref p, out w);
Vector3.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3.Dot(ref v, ref ray.Direction, out vdir);
hit.T = hit.T - vw / vdir;
if (vdir >= 0)
//We would have to back up!
{
return(false);
}
if (hit.T > maximumLength)
//If we've gone beyond where the ray can reach, there's obviously no hit.
{
return(false);
}
//Shift the ray up.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
}
//Transform the hit data into world space.
Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
//TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options.
///<summary>
/// Tests a ray against a convex shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="shape">Shape to test.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the ray cast, if any.</param>
///<returns>Whether or not the ray hit the shape.</returns>
public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
Quaternion conjugate;
Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
Vector3 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
Vector3 closestOffset = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, closestPointDotDirection;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
//If the closest offset and the extreme point->ray origin direction point the same way,
//then we might be able to conservatively advance the point towards the surface.
if (vw > 0)
{
Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
if (closestPointDotDirection >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / closestPointDotDirection;
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = closestOffset;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);
//Compute the offset from the simplex surface to the origin.
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);
}
//Transform the hit data into world space.
Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Note: When comparing this implementation with the GJK (see Gjk.cs), be aware that the
// GJK implementation computes the CSO as the Minkowski difference A-B whereas the MPR uses
// B-A. Both representations of the CSO are equivalent, we just have to invert the vectors
// here and there. (B-A was chosen because the original description of the MPR used B-A.)
if (type == CollisionQueryType.ClosestPoints)
{
throw new GeometryException("MPR cannot handle closest-point queries. Use GJK instead.");
}
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = geometricObjectA.Shape as ConvexShape;
Vector3 scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = geometricObjectB.Shape as ConvexShape;
Vector3 scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
if (shapeA == null || shapeB == null)
{
throw new ArgumentException("The contact set must contain convex shapes.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
Vector3 v0;
if (contactSet.IsPreferredNormalAvailable && type == CollisionQueryType.Contacts)
{
// Set v0, so to shoot into preferred direction.
v0 = contactSet.PreferredNormal;
// Perform only 1 MPR iteration.
DoMpr(type, contactSet, v0);
return;
}
// Find first point v0 (which determines the ray direction).
// Inner point in CSO (Minkowski difference B-A).
Vector3 v0A = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3 v0B = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
v0 = v0B - v0A;
// If v0 == origin then we have contact.
if (v0.IsNumericallyZero)
{
// The inner points overlap. Probably there are two objects centered on the same point.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return;
}
// Choose a v0 different from Zero. Any direction is ok.
// The point should still be in the Minkowski difference.
v0.X = CollisionDetection.Epsilon / 10;
}
// Call MPR in iteration until the MPR ray has converged.
int iterationCount = 0;
const int iterationLimit = 10;
Vector3 oldMprRay;
// Use a temporary contact set.
var testContactSet = ContactSet.Create(collisionObjectA, collisionObjectB);
testContactSet.IsPerturbationTestAllowed = contactSet.IsPerturbationTestAllowed;
testContactSet.PreferredNormal = contactSet.PreferredNormal;
Contact oldContact = null;
do
{
oldMprRay = v0;
if (iterationCount == 0)
{
oldMprRay.TryNormalize();
}
// Call MPR. v0 of the next iteration is simply -returned portal normal.
Debug.Assert(testContactSet.Count == 0 || testContactSet.Count == 1, "testContactSet in MPR should have 0 or 1 contacts.");
Debug.Assert(testContactSet.Count == 0 || testContactSet[0] == oldContact);
testContactSet.Clear();
// Because of numerical problems (for example with long thin ellipse vs. capsule)
// it is possible that the last iteration was a contact but in this iteration
// no contact is found. Therefore we also reset the HaveContact flag to avoid
// an end result where HaveContact is set but no Contact is in the ContactSet.
testContactSet.HaveContact = false;
v0 = -DoMpr(type, testContactSet, v0);
if (testContactSet.Count > 0)
{
var newContact = testContactSet[0];
if (oldContact != null)
{
if (oldContact.PenetrationDepth < newContact.PenetrationDepth)
{
// The new penetration depth is larger then the old penetration depth.
// In this case we keep the old contact.
// This can happen for nearly parallel boxes. First we get a good contact.
// Then we get a contact another side. Normal has changed 90�. The new
// penetration depth can be nearly the whole box side length :-(.
newContact.Recycle();
testContactSet[0] = oldContact;
break;
}
}
if (newContact != oldContact)
{
if (oldContact != null)
{
oldContact.Recycle();
}
oldContact = newContact;
}
}
iterationCount++;
} while (testContactSet.HaveContact && // Separation? - No contact which we could refine.
iterationCount < iterationLimit && // Iteration limit reached?
v0 != Vector3.Zero && // Is normal useful to go on?
!Vector3.AreNumericallyEqual(-v0, oldMprRay, CollisionDetection.Epsilon));
// Normal hasn't converged yet?
if (testContactSet.Count > 0)
{
// Recycle oldContact if not used.
if (testContactSet[0] != oldContact)
{
if (oldContact != null)
{
oldContact.Recycle();
oldContact = null;
}
}
}
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
testContactSet.Count > 0 &&
contactSet.Count < 3)
{
// Try to find full contact set.
var wrapper = TestMethodWrappers.Obtain();
wrapper.OriginalMethod = _doMprMethod;
wrapper.V0 = testContactSet[0].Normal; // The MPR ray will point along the normal of the first contact.
ContactHelper.TestWithPerturbations(
CollisionDetection,
testContactSet,
true,
wrapper.Method);
TestMethodWrappers.Recycle(wrapper);
}
contactSet.HaveContact = testContactSet.HaveContact;
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Recycle temporary objects.
testContactSet.Recycle();
}
///<summary>
/// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin.
///</summary>
///<param name="shapeA">First shape.</param>
///<param name="shapeB">Second shape.</param>
///<param name="direction">Extreme point direction in local space.</param>
///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
///<param name="extremePoint">The extreme point in the local space of A.</param>
public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
{
//Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
Vector3 extremePointB;
Vector3 negativeN;
Vector3.Negate(ref direction, out negativeN);
shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB);
Vector3.Subtract(ref extremePoint, ref extremePointB, out extremePoint);
}
protected ConvexCollidable(ConvexShape shape)
: base(shape)
{
}
///<summary> /// Initializes the pair tester. ///</summary> ///<param name="convex">Convex shape to use.</param> public abstract void Initialize(ConvexShape convex);
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
///<param name="triangle">Triangle shape to use.</param>
public override void Initialize(ConvexShape convex, TriangleShape triangle)
{
this.convex = convex;
this.triangle = triangle;
}
private List <ICollisionShape> BuildBaseAndCarShapes()
{
List <ICollisionShape> objects = new List <ICollisionShape>();
#region Terrain Base
ShapeFilename.Add("cube1.obj");
ShapeScale.Add(60);
TextureFilename.Add("texture/woodbox.bmp");
GeometryProperties geom0 = GetObjectGeometry(ShapeFilename[0], ShapeScale[0]);
var objects0 = new ConvexShape(geom0.VertexPoint, geom0.TriagleIdx, new Vector3d(0.0, -4.0, 0.0), 0.0, true);
objects0.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 1.0));
objects0.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects0.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects0.SetRestitutionCoeff(0.1);
objects0.SetDynamicFrictionCoeff(1.0);
objects0.SetStaticFrictionCoeff(1.0);
objects0.ExcludeFromCollisionDetection(false);
objects0.SetErrorReductionParam(0.7);
objects.Add(objects0);
#endregion
#region Dynamic Objects
Vector3d position = new Vector3d(0.0, 5.6, 0.0);
ShapeFilename.Add("cube2.obj");
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
GeometryProperties geom1 = GetObjectGeometry("cube2.obj", 1);
var objects_0 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 100.0, false);
objects_0.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
objects_0.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_0.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_0.SetRestitutionCoeff(0.1);
objects_0.SetDynamicFrictionCoeff(0.4);
objects_0.SetStaticFrictionCoeff(0.3);
objects_0.ExcludeFromCollisionDetection(false);
objects_0.SetErrorReductionParam(0.5);
objects.Add(objects_0);
position = new Vector3d(-1.1, 5.1, -1.5);
ShapeFilename.Add("wheel.obj");
ShapeScale.Add(0.5f);
TextureFilename.Add("texture/woodbox.bmp");
geom1 = GetObjectGeometry("wheel.obj", 0.5f);
var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.8);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
objects.Add(objects1);
position = new Vector3d(1.1, 5.1, -1.5);
ShapeFilename.Add("wheel.obj");
ShapeScale.Add(0.5f);
TextureFilename.Add("texture/woodbox.bmp");
geom1 = GetObjectGeometry("wheel.obj", 0.5f);
objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.8);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
objects.Add(objects1);
position = new Vector3d(1.1, 5.1, 1.5);
ShapeFilename.Add("wheel.obj");
ShapeScale.Add(0.5f);
TextureFilename.Add("texture/woodbox.bmp");
geom1 = GetObjectGeometry("wheel.obj", 0.5f);
objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.8);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
objects.Add(objects1);
position = new Vector3d(-1.1, 5.1, 1.5);
ShapeFilename.Add("wheel.obj");
ShapeScale.Add(0.5f);
TextureFilename.Add("texture/woodbox.bmp");
geom1 = GetObjectGeometry("wheel.obj", 0.5f);
objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.8);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
objects.Add(objects1);
#endregion
return(objects);
}
private List <ICollisionShape> BuildBridge()
{
List <ICollisionShape> objects = new List <ICollisionShape>();
Vector3d position = new Vector3d(0.0, 0.0, 12.5);
ShapeFilename.Add("cube.obj");
ShapeScale.Add(1.5f);
TextureFilename.Add("texture/woodbox.bmp");
GeometryProperties geom1 = GetObjectGeometry("cube.obj", 1.5f);
var objects_0 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 0.0, true);
objects_0.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 1.0));
objects_0.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_0.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_0.SetRestitutionCoeff(0.1);
objects_0.SetDynamicFrictionCoeff(0.4);
objects_0.SetStaticFrictionCoeff(0.4);
objects_0.ExcludeFromCollisionDetection(false);
objects_0.SetErrorReductionParam(0.3);
objects.Add(objects_0);
position = new Vector3d(0.0, 1.2, 9.5);
for (int i = 0; i < 9; i++)
{
ShapeFilename.Add("cube1.obj");
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
var geom = GetObjectGeometry("cube1.obj", 1);
var objects_1 = new ConvexShape(geom.VertexPoint, geom.TriagleIdx, position, 1.0, false);
objects_1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
objects_1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_1.SetRestitutionCoeff(0.1);
objects_1.SetDynamicFrictionCoeff(0.8);
objects_1.SetStaticFrictionCoeff(0.9);
objects_1.ExcludeFromCollisionDetection(false);
objects_1.SetErrorReductionParam(0.3);
objects.Add(objects_1);
position = position - new Vector3d(0.0, 0.0, 2.5);
}
position = new Vector3d(0.0, 0.0, -13.5);
ShapeFilename.Add("cube.obj");
ShapeScale.Add(1.5f);
TextureFilename.Add("texture/woodbox.bmp");
geom1 = GetObjectGeometry("cube.obj", 1.5f);
objects_0 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 0.0, true);
objects_0.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 1.0));
objects_0.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_0.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects_0.SetRestitutionCoeff(0.1);
objects_0.SetDynamicFrictionCoeff(0.4);
objects_0.SetStaticFrictionCoeff(0.4);
objects_0.ExcludeFromCollisionDetection(false);
objects_0.SetErrorReductionParam(0.3);
objects.Add(objects_0);
return(objects);
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
if (type == CollisionQueryType.Contacts)
{
throw new GeometryException("GJK cannot handle contact queries. Use MPR instead.");
}
IGeometricObject objectA = contactSet.ObjectA.GeometricObject;
ConvexShape shapeA = objectA.Shape as ConvexShape;
Vector3F scaleA = objectA.Scale;
Pose poseA = objectA.Pose;
IGeometricObject objectB = contactSet.ObjectB.GeometricObject;
ConvexShape shapeB = objectB.Shape as ConvexShape;
Vector3F scaleB = objectB.Scale;
Pose poseB = objectB.Pose;
if (shapeA == null || shapeB == null)
{
throw new ArgumentException("The contact set must contain two convex shapes.", "contactSet");
}
// GJK builds a simplex of the CSO (A-B). This simplex is managed in a GjkSimplexSolver.
var simplex = GjkSimplexSolver.Create();
bool foundSeparatingAxis = false;
try
{
// v is the separating axis or the CSO point nearest to the origin.
// We start with last known separating axis or with an arbitrary CSO point.
Vector3F v;
if (contactSet.Count > 0)
{
// Use last separating axis.
// The contact normal points from A to B. This is the direction we want to sample first.
// If the frame-to-frame coherence is high we should get a point close to the origin.
// Note: To sample in the normal direction, we need to initialize the CSO point v with
// -normal.
v = -contactSet[0].Normal;
}
else
{
// Use difference of inner points.
Vector3F vA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
Vector3F vB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
v = vA - vB;
}
// If the inner points overlap, then we have already found a contact.
// We don't expect this case to happen often, so we simply choose an arbitrary separating
// axis to continue with the normal GJK code.
if (v.IsNumericallyZero)
{
v = Vector3F.UnitZ;
}
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
int iterationCount = 0;
float distanceSquared = float.MaxValue;
float distanceEpsilon;
// Assume we have no contact.
contactSet.HaveContact = false;
do
{
// TODO: Translate A and B close to the origin to avoid numerical problems.
// This optimization is done in Bullet: The offset (a.Pose.Position + b.Pose.Position) / 2
// is subtracted from a.Pose and b.Pose. This offset is added when the Contact info is
// computed (also in EPA if the poses are still translated).
// Compute a new point w on the simplex. We seek for the point that is closest to the origin.
// Therefore, we get the support points on the current separating axis v.
Vector3F p = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -v, scaleA));
Vector3F q = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * v, scaleB));
Vector3F w = p - q;
// w projected onto the separating axis.
float delta = Vector3F.Dot(w, v);
// If v∙w > 0 then the objects do not overlap.
if (delta > 0)
{
// We have found a separating axis.
foundSeparatingAxis = true;
// Early exit for boolean and contact queries.
if (type == CollisionQueryType.Boolean || type == CollisionQueryType.Contacts)
{
// TODO: We could cache the separating axis n in ContactSet for future collision checks.
return;
}
// We continue for closest point queries because we don't know if there are other
// points closer than p and q.
}
// If the new w is already part of the simplex. We cannot improve any further.
if (simplex.Contains(w))
{
break;
}
// If the new w is not closer to the origin (within numerical tolerance), we stop.
if (distanceSquared - delta <= distanceSquared * Numeric.EpsilonF) // SOLID uses Epsilon = 10^-6
{
break;
}
// Add the new point to the simplex.
simplex.Add(w, p, q);
// Update the simplex. (Unneeded simplex points are removed).
simplex.Update();
// Get new point of simplex closest to the origin.
v = simplex.ClosestPoint;
float previousDistanceSquared = distanceSquared;
distanceSquared = v.LengthSquared;
if (previousDistanceSquared < distanceSquared)
{
// If the result got worse, we use the previous result. This happens for
// degenerate cases for example when the simplex is a tetrahedron with all
// 4 vertices in a plane.
distanceSquared = previousDistanceSquared;
simplex.Backup();
break;
}
// If the new simplex is invalid, we stop.
// Example: A simplex gets invalid if a fourth vertex is added to create a tetrahedron
// simplex but all vertices are in a plane. This can happen if a box corner nearly touches a
// face of another box.
if (!simplex.IsValid)
{
break;
}
// Compare the distance of v to the origin with the distance of the last iteration.
// We stop if the improvement is less than the numerical tolerance.
if (previousDistanceSquared - distanceSquared <= previousDistanceSquared * Numeric.EpsilonF)
{
break;
}
// If we reach the iteration limit, we stop.
iterationCount++;
if (iterationCount > MaxNumberOfIterations)
{
Debug.Assert(false, "GJK reached the iteration limit.");
break;
}
// Compute a scaled epsilon.
distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
// Loop until the simplex is full (i.e. it contains the origin) or we have come
// sufficiently close to the origin.
} while (!simplex.IsFull && distanceSquared > distanceEpsilon);
Debug.Assert(simplex.IsEmpty == false, "The GJK simplex must contain at least 1 point.");
// Compute contact normal and separation.
Vector3F normal = -simplex.ClosestPoint; // simplex.ClosestPoint = ClosestPointA-ClosestPointB
float distance;
distanceEpsilon = Numeric.EpsilonFSquared * Math.Max(1, simplex.MaxVertexDistanceSquared);
if (distanceSquared <= distanceEpsilon)
{
// Distance is approximately 0.
// --> Objects are in contact.
if (simplex.IsValid && normal.TryNormalize())
{
// Normal can be used but we have to invert it because for contact we
// have to compute normal as pointOnA - pointOnB.
normal = -normal;
}
else
{
// No useful normal. Use direction between inner points as a fallback.
Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
normal = simplex.ClosestPointOnA - innerA;
if (!normal.TryNormalize())
{
Vector3F innerB = poseB.ToWorldPosition(shapeB.InnerPoint * scaleB);
normal = innerB - innerA;
if (!normal.TryNormalize())
{
normal = Vector3F.UnitY;
// TODO: We could use better normal: e.g. normal of old contact or PreferredNormal?
}
}
}
distance = 0;
contactSet.HaveContact = true;
}
else
{
// Distance is greater than 0.
distance = (float)Math.Sqrt(distanceSquared);
normal /= distance;
// If the simplex is valid and full, then we have a contact.
if (simplex.IsFull && simplex.IsValid)
{
// Let's use the current result as an estimated contact info for
// shallow contacts.
// TODO: The following IF was added because this can occur for valid
// triangle vs. triangle separation. Check this.
if (!foundSeparatingAxis)
{
contactSet.HaveContact = true;
// Distance is a penetration depth
distance = -distance;
// Since the simplex tetrahedron can have any position in the Minkowsky difference,
// we do not know the real normal. Let's use the current normal and make
// sure that it points away from A. - This is only a heuristic...
Vector3F innerA = poseA.ToWorldPosition(shapeA.InnerPoint * scaleA);
if (Vector3F.Dot(simplex.ClosestPointOnA - innerA, normal) < 0)
{
normal = -normal;
}
}
}
}
Debug.Assert(normal.IsNumericallyZero == false);
if (type != CollisionQueryType.Boolean)
{
Vector3F position = (simplex.ClosestPointOnA + simplex.ClosestPointOnB) / 2;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, -distance, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
finally
{
simplex.Recycle();
}
}
protected ConvexCollidable(ConvexShape shape)
: base(shape)
{
Events = new ContactEventManager <EntityCollidable>();
}
///<summary>
/// Sweeps a shape against another shape using a given sweep vector.
///</summary>
///<param name="sweptShape">Shape to sweep.</param>
///<param name="target">Shape being swept against.</param>
///<param name="sweep">Sweep vector for the sweptShape.</param>
///<param name="startingSweptTransform">Starting transform of the sweptShape.</param>
///<param name="targetTransform">Transform to apply to the target shape.</param>
///<param name="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shape hit the other shape.</returns>
public static bool ConvexCast(ConvexShape sweptShape, ConvexShape target, ref Vector3 sweep, ref RigidTransform startingSweptTransform, ref RigidTransform targetTransform,
out RayHit hit)
{
return(ConvexCast(sweptShape, target, ref sweep, ref Toolbox.ZeroVector, ref startingSweptTransform, ref targetTransform, out hit));
}
public override bool ConvexCast(ConvexShape castShape, ref MathExtensions.RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
return(MPRToolbox.Sweep(castShape, Shape, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref worldTransform, out hit));
}
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
return(ConvexCast(castShape, ref startingTransform, ref sweep, null, out hit));
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the plane.
// Object B should be the other object.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap objects if necessary.
bool swapped = (convexObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref convexObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (planeShape == null || convexShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a convex shape.", "contactSet");
}
// Get transformations.
Vector3F scalePlane = planeObject.Scale;
Vector3F scaleB = convexObject.Scale;
Pose planePose = planeObject.Pose;
Pose poseB = convexObject.Pose;
// Apply scale to plane and transform plane into world space.
Plane planeWorld = new Plane(planeShape);
planeWorld.Scale(ref scalePlane); // Scale plane.
planeWorld.ToWorld(ref planePose); // Transform plane to world space.
// Transform plane normal to local space of convex.
Vector3F planeNormalLocalB = poseB.ToLocalDirection(planeWorld.Normal);
// Get support vertex nearest to the plane.
Vector3F supportVertexBLocal = convexShape.GetSupportPoint(-planeNormalLocalB, scaleB);
// Transform support vertex into world space.
Vector3F supportVertexBWorld = poseB.ToWorldPosition(supportVertexBLocal);
// Project vertex onto separating axis (given by plane normal).
float distance = Vector3F.Dot(supportVertexBWorld, planeWorld.Normal);
// Check for collision.
float penetrationDepth = planeWorld.DistanceFromOrigin - distance;
contactSet.HaveContact = (penetrationDepth >= 0);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// Position is between support vertex and plane.
Vector3F position = supportVertexBWorld + planeWorld.Normal * (penetrationDepth / 2);
Vector3F normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
contactSet.Count > 0 &&
contactSet.Count < 4)
{
// Special treatment for tetrahedra: Test all vertices against plane.
IList <Vector3F> vertices = null;
if (convexShape is ConvexHullOfPoints)
{
var convexHullOfPoints = (ConvexHullOfPoints)convexShape;
vertices = convexHullOfPoints.Points;
}
else if (convexShape is ConvexPolyhedron)
{
var convexPolyhedron = (ConvexPolyhedron)convexShape;
vertices = convexPolyhedron.Vertices;
}
if (vertices != null && vertices.Count <= 8)
{
// Convex has 8 or less vertices. Explicitly test all vertices against the plane.
int numberOfVertices = vertices.Count;
for (int i = 0; i < numberOfVertices; i++)
{
// Test is the same as above.
var vertex = vertices[i];
Vector3F scaledVertex = vertex * scaleB;
if (scaledVertex != supportVertexBLocal) // supportVertexBLocal has already been added.
{
Vector3F vertexWorld = poseB.ToWorldPosition(scaledVertex);
distance = Vector3F.Dot(vertexWorld, planeWorld.Normal);
penetrationDepth = planeWorld.DistanceFromOrigin - distance;
if (penetrationDepth >= 0)
{
position = vertexWorld + planeWorld.Normal * (penetrationDepth / 2);
normal = (swapped) ? -planeWorld.Normal : planeWorld.Normal;
contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
}
}
}
else
{
// Convex is a complex shape with more than 4 vertices.
ContactHelper.TestWithPerturbations(
CollisionDetection,
contactSet,
!swapped, // Perturb the convex object, not the plane.
_computeContactsMethod);
}
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Ray vs. convex has at max 1 contact.
Debug.Assert(contactSet.Count <= 1);
// Object A should be the ray.
// Object B should be the convex.
IGeometricObject rayObject = contactSet.ObjectA.GeometricObject;
IGeometricObject convexObject = contactSet.ObjectB.GeometricObject;
// Swap object if necessary.
bool swapped = (convexObject.Shape is RayShape);
if (swapped)
{
MathHelper.Swap(ref rayObject, ref convexObject);
}
RayShape rayShape = rayObject.Shape as RayShape;
ConvexShape convexShape = convexObject.Shape as ConvexShape;
// Check if shapes are correct.
if (rayShape == null || convexShape == null)
{
throw new ArgumentException("The contact set must contain a ray and a convex shape.", "contactSet");
}
// Call line segment vs. convex for closest points queries.
if (type == CollisionQueryType.ClosestPoints)
{
// Find point on ray closest to the convex shape.
// Call GJK.
_gjk.ComputeCollision(contactSet, type);
if (contactSet.HaveContact == false)
{
return;
}
// Otherwise compute 1 contact ...
// GJK result is invalid for penetration.
foreach (var contact in contactSet)
{
contact.Recycle();
}
contactSet.Clear();
}
// Assume no contact.
contactSet.HaveContact = false;
// Get transformations.
Vector3 rayScale = rayObject.Scale;
Vector3 convexScale = convexObject.Scale;
Pose convexPose = convexObject.Pose;
Pose rayPose = rayObject.Pose;
// See Raycasting paper of van den Bergen or Bullet.
// Note: Compute in local space of convex (object B).
// Scale ray and transform ray to local space of convex.
Ray rayWorld = new Ray(rayShape);
rayWorld.Scale(ref rayScale); // Scale ray.
rayWorld.ToWorld(ref rayPose); // Transform ray to world space.
Ray ray = rayWorld;
ray.ToLocal(ref convexPose); // Transform ray to local space of convex.
var simplex = GjkSimplexSolver.Create();
try
{
Vector3 s = ray.Origin; // source
Vector3 r = ray.Direction * ray.Length; // ray
float λ = 0; // ray parameter
Vector3 x = s; // hit spot (on ray)
Vector3 n = new Vector3(); // normal
Vector3 v = x - convexShape.GetSupportPoint(ray.Direction, convexScale);
// v = x - arbitrary point. Vector used for support mapping.
float distanceSquared = v.LengthSquared(); // ||v||²
int iterationCount = 0;
while (distanceSquared > Numeric.EpsilonF && iterationCount < MaxNumberOfIterations)
{
iterationCount++;
Vector3 p = convexShape.GetSupportPoint(v, convexScale); // point on convex
Vector3 w = x - p; // simplex/Minkowski difference point
float vDotW = Vector3.Dot(v, w); // v∙w
if (vDotW > 0)
{
float vDotR = Vector3.Dot(v, r); // v∙r
if (vDotR >= 0) // TODO: vDotR >= - Epsilon^2 ?
{
return; // No Hit.
}
λ = λ - vDotW / vDotR;
x = s + λ * r;
simplex.Clear(); // Configuration space obstacle (CSO) is translated whenever x is updated.
w = x - p;
n = v;
}
simplex.Add(w, x, p);
simplex.Update();
v = simplex.ClosestPoint;
distanceSquared = (simplex.IsValid && !simplex.IsFull) ? v.LengthSquared() : 0;
}
// We have a contact if the hit is inside the ray length.
contactSet.HaveContact = (0 <= λ && λ <= 1);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
float penetrationDepth = λ * ray.Length;
Debug.Assert(contactSet.HaveContact, "Separation was not detected by GJK above.");
// Convert back to world space.
Vector3 position = rayWorld.Origin + rayWorld.Direction * penetrationDepth;
n = convexPose.ToWorldDirection(n);
if (!n.TryNormalize())
{
n = Vector3.UnitY;
}
if (swapped)
{
n = -n;
}
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, -n, penetrationDepth, true);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
finally
{
simplex.Recycle();
}
}
private Vector3 DoMpr(CollisionQueryType type, ContactSet contactSet, Vector3 v0)
{
int iterationCount = 0;
const int iterationLimit = 100;
CollisionObject collisionObjectA = contactSet.ObjectA;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
ConvexShape shapeA = (ConvexShape)geometricObjectA.Shape;
Vector3 scaleA = geometricObjectA.Scale;
Pose poseA = geometricObjectA.Pose;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
ConvexShape shapeB = (ConvexShape)geometricObjectB.Shape;
Vector3 scaleB = geometricObjectB.Scale;
Pose poseB = geometricObjectB.Pose;
// Cache inverted rotations.
var orientationAInverse = poseA.Orientation.Transposed;
var orientationBInverse = poseB.Orientation.Transposed;
Vector3 n = -v0; // Shoot from v0 to the origin.
Vector3 v1A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3 v1B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3 v1 = v1B - v1A;
// Separating axis test:
if (Vector3.Dot(v1, n) < 0)
{
// TODO: We could cache the separating axis n in ContactSet for future collision checks.
// Also in the separating axis tests below.
return(Vector3.Zero);
}
// Second support direction = perpendicular to plane of origin, v0 and v1.
n = Vector3.Cross(v1, v0);
// If n is a zero vector, then origin, v0 and v1 are on a line with the origin inside the support plane.
if (n.IsNumericallyZero)
{
// Contact found.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
// Compute contact information.
// (v0 is an inner point. v1 is a support point on the CSO. => The contact normal is -v1.
// However, v1 could be close to the origin. To avoid numerical
// problems we use v0 - v1, which is the same direction.)
Vector3 normal = v0 - v1;
if (!normal.TryNormalize())
{
// This happens for Point vs. flat object when they are on the same position.
// Maybe we could even find a better normal.
normal = Vector3.UnitY;
}
Vector3 position = (v1A + v1B) / 2;
float penetrationDepth = v1.Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return(Vector3.Zero);
}
Vector3 v2A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
Vector3 v2B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
Vector3 v2 = v2B - v2A;
// Separating axis test:
if (Vector3.Dot(v2, n) < 0)
{
return(Vector3.Zero);
}
// Third support direction = perpendicular to plane of v0, v1 and v2.
n = Vector3.Cross(v1 - v0, v2 - v0);
// If the origin is on the negative side of the plane, then reverse the plane direction.
// n must point into the origin direction and not away...
if (Vector3.Dot(n, v0) > 0)
{
MathHelper.Swap(ref v1, ref v2);
MathHelper.Swap(ref v1A, ref v2A);
MathHelper.Swap(ref v1B, ref v2B);
n = -n;
}
if (n.IsNumericallyZero)
{
// Degenerate case:
// Interpretation (HelmutG): v2 is on the line with v0 and v1. I think this can only happen
// if the CSO is flat and in the plane of (origin, v0, v1).
// This happens for example in Point vs. Line Segment, or triangle vs. triangle when both
// triangles are in the same plane.
// Simply ignore this case (Infinite small/flat objects do not touch).
return(Vector3.Zero);
}
// Search for a valid portal.
Vector3 v3, v3A, v3B;
while (true)
{
iterationCount++;
// Abort if we cannot find a valid portal.
if (iterationCount > iterationLimit)
{
return(Vector3.Zero);
}
// Get next support point.
//v3A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
//v3B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
//v3 = v3B - v3A;
// ----- Optimized version:
Vector3 supportDirectionA;
supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
v3A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
v3A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
v3A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
Vector3 supportDirectionB;
supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
v3B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
v3B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
v3B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
v3 = v3B - v3A;
// Separating axis test:
//if (Vector3.Dot(v3, n) < 0)
if (v3.X * n.X + v3.Y * n.Y + v3.Z * n.Z < 0)
{
return(Vector3.Zero);
}
// v0, v1, v2, v3 form a tetrahedron.
// v0 is an inner point of the CSO and v1, v2, v3 are support points.
// v1, v2, v3 should form a valid portal.
// If origin is outside the plane of v0, v1, v3 then the portal is invalid and we choose a new n.
//if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0) // ORIENT3D test, see Ericson: "Real-Time Collision Detection"
if ((v1.Y * v3.Z - v1.Z * v3.Y) * v0.X
+ (v1.Z * v3.X - v1.X * v3.Z) * v0.Y
+ (v1.X * v3.Y - v1.Y * v3.X) * v0.Z < 0)
{
v2 = v3; // Get rid of v2. A new v3 will be chosen in the next iteration.
v2A = v3A;
v2B = v3B;
//n = Vector3.Cross(v1 - v0, v3 - v0);
// ----- Optimized version:
Vector3 v1MinusV0;
v1MinusV0.X = v1.X - v0.X;
v1MinusV0.Y = v1.Y - v0.Y;
v1MinusV0.Z = v1.Z - v0.Z;
Vector3 v3MinusV0;
v3MinusV0.X = v3.X - v0.X;
v3MinusV0.Y = v3.Y - v0.Y;
v3MinusV0.Z = v3.Z - v0.Z;
n.X = v1MinusV0.Y * v3MinusV0.Z - v1MinusV0.Z * v3MinusV0.Y;
n.Y = v1MinusV0.Z * v3MinusV0.X - v1MinusV0.X * v3MinusV0.Z;
n.Z = v1MinusV0.X * v3MinusV0.Y - v1MinusV0.Y * v3MinusV0.X;
continue;
}
// If origin is outside the plane of v0, v2, v3 then the portal is invalid and we choose a new n.
//if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0)
if ((v3.Y * v2.Z - v3.Z * v2.Y) * v0.X
+ (v3.Z * v2.X - v3.X * v2.Z) * v0.Y
+ (v3.X * v2.Y - v3.Y * v2.X) * v0.Z < 0)
{
v1 = v3; // Get rid of v1. A new v3 will be chosen in the next iteration.
v1A = v3A;
v1B = v3B;
//n = Vector3.Cross(v3 - v0, v2 - v0);
// ----- Optimized version:
Vector3 v3MinusV0;
v3MinusV0.X = v3.X - v0.X;
v3MinusV0.Y = v3.Y - v0.Y;
v3MinusV0.Z = v3.Z - v0.Z;
Vector3 v2MinusV0;
v2MinusV0.X = v2.X - v0.X;
v2MinusV0.Y = v2.Y - v0.Y;
v2MinusV0.Z = v2.Z - v0.Z;
n.X = v3MinusV0.Y * v2MinusV0.Z - v3MinusV0.Z * v2MinusV0.Y;
n.Y = v3MinusV0.Z * v2MinusV0.X - v3MinusV0.X * v2MinusV0.Z;
n.Z = v3MinusV0.X * v2MinusV0.Y - v3MinusV0.Y * v2MinusV0.X;
continue;
}
// If come to here, then we have found a valid portal to begin with.
// (We have a tetrahedron that contains the ray (v0 to origin)).
break;
}
// Refine the portal
while (true)
{
iterationCount++;
// Store old n. Numerical inaccuracy can lead to endless loops where n is constant.
Vector3 oldN = n;
// Compute outward pointing normal of the portal
//n = Vector3.Cross(v2 - v1, v3 - v1);
Vector3 v2MinusV1;
v2MinusV1.X = v2.X - v1.X;
v2MinusV1.Y = v2.Y - v1.Y;
v2MinusV1.Z = v2.Z - v1.Z;
Vector3 v3MinusV1;
v3MinusV1.X = v3.X - v1.X;
v3MinusV1.Y = v3.Y - v1.Y;
v3MinusV1.Z = v3.Z - v1.Z;
n.X = v2MinusV1.Y * v3MinusV1.Z - v2MinusV1.Z * v3MinusV1.Y;
n.Y = v2MinusV1.Z * v3MinusV1.X - v2MinusV1.X * v3MinusV1.Z;
n.Z = v2MinusV1.X * v3MinusV1.Y - v2MinusV1.Y * v3MinusV1.X;
//if (!n.TryNormalize())
// ----- Optimized version:
float nLengthSquared = n.LengthSquared();
if (nLengthSquared < Numeric.EpsilonFSquared)
{
// The portal is degenerate (some vertices of v1, v2, v3 are identical).
// This can happen for coplanar shapes, e.g. long thin triangles in the
// same plane. The portal (v1, v2, v3) is a line segment.
// This might be a contact or not. We use the GJK as a fallback to check this case.
if (_gjk == null)
{
_gjk = new Gjk(CollisionDetection);
}
_gjk.ComputeCollision(contactSet, CollisionQueryType.Boolean);
if (contactSet.HaveContact == false)
{
return(Vector3.Zero);
}
// GJK reports a contact - but it cannot compute contact positions.
// We use the best point on the current portal as the contact point.
// Find the point closest to the origin.
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);
Vector3 vClosest = u * v1 + v * v2 + w * v3;
// We have not found a separating axis so far. --> Contact.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
// The points on the objects have the same barycentric coordinates.
Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
Vector3 pointOnB = u * v1B + v * v2B + w * v3B;
Vector3 normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
if (contactSet.IsPreferredNormalAvailable)
{
normal = contactSet.PreferredNormal;
}
else
{
normal = Vector3.UnitY;
}
}
Vector3 position = (pointOnA + pointOnB) / 2;
float penetrationDepth = vClosest.Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
return(Vector3.Zero);
}
// ----- Optimized version: Rest of n.TryNormalize():
float nLength = (float)Math.Sqrt(nLengthSquared);
float scale = 1.0f / nLength;
n.X *= scale;
n.Y *= scale;
n.Z *= scale;
// Separating axis test:
// Testing > instead of >= is important otherwise coplanar triangles may report false contacts
// because the portal is in the same plane as the origin.
if (!contactSet.HaveContact &&
v1.X * n.X + v1.Y * n.Y + v1.Z * n.Z > 0) // Optimized version of && Vector3.Dot(v1, n) > 0)
{
// Portal points aways from origin --> Origin is in the tetrahedron.
contactSet.HaveContact = true;
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
}
// Find new support point.
//Vector3 v4A = poseA.ToWorldPosition(shapeA.GetSupportPoint(orientationAInverse * -n, scaleA));
//Vector3 v4B = poseB.ToWorldPosition(shapeB.GetSupportPoint(orientationBInverse * n, scaleB));
//Vector3 v4 = v4B - v4A;
// ----- Optimized version:
Vector3 supportDirectionA;
supportDirectionA.X = -(orientationAInverse.M00 * n.X + orientationAInverse.M01 * n.Y + orientationAInverse.M02 * n.Z);
supportDirectionA.Y = -(orientationAInverse.M10 * n.X + orientationAInverse.M11 * n.Y + orientationAInverse.M12 * n.Z);
supportDirectionA.Z = -(orientationAInverse.M20 * n.X + orientationAInverse.M21 * n.Y + orientationAInverse.M22 * n.Z);
Vector3 supportPointA = shapeA.GetSupportPoint(supportDirectionA, scaleA);
Vector3 v4A;
v4A.X = poseA.Orientation.M00 * supportPointA.X + poseA.Orientation.M01 * supportPointA.Y + poseA.Orientation.M02 * supportPointA.Z + poseA.Position.X;
v4A.Y = poseA.Orientation.M10 * supportPointA.X + poseA.Orientation.M11 * supportPointA.Y + poseA.Orientation.M12 * supportPointA.Z + poseA.Position.Y;
v4A.Z = poseA.Orientation.M20 * supportPointA.X + poseA.Orientation.M21 * supportPointA.Y + poseA.Orientation.M22 * supportPointA.Z + poseA.Position.Z;
Vector3 supportDirectionB;
supportDirectionB.X = orientationBInverse.M00 * n.X + orientationBInverse.M01 * n.Y + orientationBInverse.M02 * n.Z;
supportDirectionB.Y = orientationBInverse.M10 * n.X + orientationBInverse.M11 * n.Y + orientationBInverse.M12 * n.Z;
supportDirectionB.Z = orientationBInverse.M20 * n.X + orientationBInverse.M21 * n.Y + orientationBInverse.M22 * n.Z;
Vector3 supportPointB = shapeB.GetSupportPoint(supportDirectionB, scaleB);
Vector3 v4B;
v4B.X = poseB.Orientation.M00 * supportPointB.X + poseB.Orientation.M01 * supportPointB.Y + poseB.Orientation.M02 * supportPointB.Z + poseB.Position.X;
v4B.Y = poseB.Orientation.M10 * supportPointB.X + poseB.Orientation.M11 * supportPointB.Y + poseB.Orientation.M12 * supportPointB.Z + poseB.Position.Y;
v4B.Z = poseB.Orientation.M20 * supportPointB.X + poseB.Orientation.M21 * supportPointB.Y + poseB.Orientation.M22 * supportPointB.Z + poseB.Position.Z;
Vector3 v4 = v4B - v4A;
// Separating axis test:
if (!contactSet.HaveContact && // <--- New (see below).
v4.X * n.X + v4.Y * n.Y + v4.Z * n.Z < 0) // Optimized version of && Vector3.Dot(v4, n) < 0)
{
// Following assert can fail. For example if the above dot product returns -0.000000001
// for nearly perfectly touching objects. Therefore I have added the condition
// hit == false to the condition.
return(Vector3.Zero);
}
// Test if we have refined more than the collision epsilon.
// Condition 1: Project the point difference v4-v3 onto normal n and check whether we have
// improved in this direction.
// Condition 2: If n has not changed, then we couldn't improve anymore. This is caused
// by numerical problems, e.g. when a large object (>10000) is checked.
//if (Vector3.Dot(v4 - v3, n) <= CollisionDetection.Epsilon
// ----- Optimized version:
if ((v4.X - v3.X) * n.X + (v4.Y - v3.Y) * n.Y + (v4.Z - v3.Z) * n.Z <= CollisionDetection.Epsilon ||
Vector3.AreNumericallyEqual(n, oldN) ||
iterationCount >= iterationLimit)
{
// We have the final portal.
if (!contactSet.HaveContact)
{
return(Vector3.Zero);
}
if (type == CollisionQueryType.Boolean)
{
return(Vector3.Zero);
}
// Find the point closest to the origin.
float u, v, w;
GeometryHelper.GetClosestPoint(new Triangle(v1, v2, v3), Vector3.Zero, out u, out v, out w);
// Note: If u, v or w is 0 or 1, then the point was probably outside portal triangle.
// We can use the returned data, but re-running MPR will give us a better contact.
Vector3 closest = u * v1 + v * v2 + w * v3;
// The points on the objects have the same barycentric coordinates.
Vector3 pointOnA = u * v1A + v * v2A + w * v3A;
Vector3 pointOnB = u * v1B + v * v2B + w * v3B;
// Use difference between points as normal direction, only if it can be normalized.
Vector3 normal = pointOnA - pointOnB;
if (!normal.TryNormalize())
{
normal = -n; // Else use the inverted normal of the portal.
}
Vector3 position = (pointOnA + pointOnB) / 2;
float penetrationDepth = closest.Length;
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
// If real closest point is outside the portal triangle, then one of u, v, w will
// be exactly 0 or 1. In this case we should run a new MPR with the portal ray n.
if (u == 0 || v == 0 || w == 0 || u == 1 || v == 1 || w == 1)
{
return(n);
}
return(Vector3.Zero);
}
// Now we have a new point v4 and have to make a new portal by eliminating v1, v2 or v3.
// The possible new tetrahedron faces are: (v0, v1, v4), (v0, v4, v2), (v0, v4, v3)
// We don't know the orientation yet.
// Test with the ORIENT3D test.
//Vector3 cross = Vector3.Cross(v4, v0);
// ----- Optimized version:
Vector3 cross;
cross.X = v4.Y * v0.Z - v4.Z * v0.Y;
cross.Y = v4.Z * v0.X - v4.X * v0.Z;
cross.Z = v4.X * v0.Y - v4.Y * v0.X;
//if (Vector3.Dot(v1, cross) > 0)
if (v1.X * cross.X + v1.Y * cross.Y + v1.Z * cross.Z > 0)
{
// Eliminate v3 or v1.
//if (Vector3.Dot(v2, cross) > 0)
if (v2.X * cross.X + v2.Y * cross.Y + v2.Z * cross.Z > 0)
{
v1 = v4;
v1A = v4A;
v1B = v4B;
}
else
{
v3 = v4;
v3A = v4A;
v3B = v4B;
}
}
else
{
// Eliminate v1 or v2.
//if (Vector3.Dot(v3, cross) > 0)
if (v3.X * cross.X + v3.Y * cross.Y + v3.Z * cross.Z > 0)
{
v2 = v4;
v2A = v4A;
v2B = v4B;
}
else
{
v1 = v4;
v1A = v4A;
v1B = v4B;
}
}
}
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
{
Vector3 negativeDirection;
Vector3.Negate(ref closestPoint, out negativeDirection);
Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
Vector3 S;
Vector3.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
Fix64 dotS;
Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
Fix64 distanceToClosest = closestPoint.LengthSquared();
Fix64 progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
{
return(true);
}
if (distanceToClosest < previousDistanceToClosest)
{
previousDistanceToClosest = distanceToClosest;
}
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return(false);
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return(false);
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return(false);
}
return(false);
}
///<summary>
/// Sweeps two shapes against another.
///</summary>
///<param name="shapeA">First shape being swept.</param>
///<param name="shapeB">Second shape being swept.</param>
///<param name="sweepA">Sweep vector for the first shape.</param>
///<param name="sweepB">Sweep vector for the second shape.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shapes hit each other..</returns>
public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB,
ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
Vector3 velocityWorld;
Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld);
Quaternion conjugateOrientationA;
Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
Vector3 rayDirection;
Quaternion.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
//Transform b into a's local space.
RigidTransform localTransformB;
Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA,
out localTransformB.Orientation);
Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
Vector3 w, p;
hit.T = 0;
hit.Location = Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
do
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);
Vector3.Subtract(ref hit.Location, ref p, out w);
Vector3.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3.Dot(ref v, ref rayDirection, out vdir);
if (vdir >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / vdir;
if (hit.T > 1)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3.Multiply(ref rayDirection, hit.T, out hit.Location);
//The ray origin is the origin! Don't need to add any ray position.
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
//Could measure the progress of the ray. If it's too little, could early out.
//Not used by default since it's biased towards precision over performance.
} while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
//Transform the hit data into world space.
Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location);
return(true);
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
public override void Initialize(ConvexShape convex)
{
this.convex = convex;
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
public override void Initialize(ConvexShape convex)
{
this.sphere = (SphereShape)convex;
}
private List <ICollisionShape> BuildStack()
{
List <ICollisionShape> objects = new List <ICollisionShape>();
Vector3d shift = new Vector3d(0.0, 2.3, 0.0);
Vector3d position = new Vector3d(8.0, 4.0, 0.0);
string objName = "cube.obj";
double[] mass = new double[] { 50, 20, 8, 3, 1 };
GeometryProperties geom1 = GetObjectGeometry(objName, 1.0f);
ShapeGeometry shapeGeometry = new ShapeGeometry(geom1.VertexPoint, geom1.TriagleIdx);
//ShapeGeometry shapeGeometry = new ShapeGeometry(geom1.VertexPoint);
for (int i = 0; i < 15; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1.0f);
TextureFilename.Add("texture/woodbox.bmp");
//var objects1 = new ConcaveShape(shapeGeometry, position, 1.0, false);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 1.0));
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(11.0, 4.0, 0.0);
for (int i = 0; i < 4; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(5.0, 1.7, 0.0);
for (int i = 0; i < 4; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(5.0, 1.7, 3.0);
for (int i = 0; i < 4; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(5.0, 1.7, -5.0);
for (int i = 0; i < 4; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(5.0, 1.7, 8.0);
for (int i = 0; i < 5; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(8.0, 1.7, -3.0);
for (int i = 0; i < 10; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
position = new Vector3d(8.0, 1.7, 3.0);
for (int i = 0; i < 10; i++)
{
ShapeFilename.Add(objName);
ShapeScale.Add(1);
TextureFilename.Add("texture/woodbox.bmp");
//GeometryProperties geom1 = GetObjectGeometry(objName, 1, 0.0);
//var objects1 = new ConvexShape(geom1.VertexPoint, geom1.TriagleIdx, position, 1.0);
var objects1 = new ConvexShape(shapeGeometry, position, 1.0, false);
objects1.SetRotationStatus(new Quaternion(new Vector3d(0.0, 0.0, 0.0), 0.0));
//objects1.SetGeometry(geom1.VertexPoint, geom1.TriagleIdx);
objects1.SetLinearVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetAngularVelocity(new Vector3d(0.0, 0.0, 0.0));
objects1.SetRestitutionCoeff(0.1);
objects1.SetDynamicFrictionCoeff(0.3);
objects1.SetStaticFrictionCoeff(0.9);
objects1.ExcludeFromCollisionDetection(false);
objects1.SetErrorReductionParam(0.3);
position = position + shift;
objects.Add(objects1);
}
return(objects);
}
