public BasicDemo()
{
SphereBoxCollisionAlgorithm.CreateFunc boxAlgo = new SphereBoxCollisionAlgorithm.CreateFunc();
boxAlgo.IsSwapped = true;
CollisionDispatcher.RegisterCollisionCreateFunc(BroadphaseNativeTypes.Sphere, BroadphaseNativeTypes.Sphere, new SphereSphereCollisionAlgorithm.CreateFunc());
CollisionDispatcher.RegisterCollisionCreateFunc(BroadphaseNativeTypes.Sphere, BroadphaseNativeTypes.Box, new SphereBoxCollisionAlgorithm.CreateFunc());
CollisionDispatcher.RegisterCollisionCreateFunc(BroadphaseNativeTypes.Box, BroadphaseNativeTypes.Sphere, boxAlgo);
Shapes[0] = new SphereShape(50);
Shapes[2] = new SphereShape(HalfExtents - CollisionMargin);
Matrix tr = Matrix.Identity;
tr.Translation = new Vector3(0, -50, 0);
CreateRigidBody(0, tr, Shapes[0]);
for (int i = 0; i < NumObjects; i++)
{
Shapes[2].Margin = CollisionMargin;
int colsize = 2;
int row = (int)((i * HalfExtents * 2) / (colsize * 2 * HalfExtents));
int row2 = row;
int col = i % colsize - colsize / 2;
tr.Translation = new Vector3(col * 2 * HalfExtents + (row2 % 2) * HalfExtents,
row * 2 * HalfExtents + HalfExtents, 0);
CreateRigidBody(1, tr, Shapes[2]);
}
}
public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
{
this._sphere = sphere;
this._triangle = triangle;
}
public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity,
OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion orientation)
: base(avName)
{
//This fields will be removed. They're temporal
float _sizeX = 0.5f;
float _sizeY = 0.5f;
float _sizeZ = 1.6f;
//.
_position = pos;
_velocity = velocity;
_size = size;
//---
_size.X = _sizeX;
_size.Y = _sizeY;
_size.Z = _sizeZ;
//.
_acceleration = acceleration;
_orientation = orientation;
_physical = true;
float _mass = 50.0f; //This depends of avatar's dimensions
//For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 0.5f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
lock (BulletXScene.BulletXLock)
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
//CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f));
//For now, like ODE, collisionShape = sphere of radious = 1.0
CollisionShape _collisionShape = new SphereShape(1.0f);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
rigidBody =
new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
_friction, _restitution);
//rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
rigidBody.Translate(_vDebugTranslation);
parent_scene.ddWorld.AddRigidBody(rigidBody);
}
}
public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
//Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
//col0->m_worldTransform,
float resultFraction = 1f;
float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
if (squareMotA < colA.CcdSquareMotionThreshold &&
squareMotB < colB.CcdSquareMotionThreshold)
return resultFraction;
if (DisableCcd)
return 1f;
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
// Convex0 against sphere for Convex1
{
ConvexShape convexA = colA.CollisionShape as ConvexShape;
SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
//Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
colA.HitFraction = result.Fraction;
if (colB.HitFraction > result.Fraction)
colB.HitFraction = result.Fraction;
if (resultFraction > result.Fraction)
resultFraction = result.Fraction;
}
}
// Sphere (for convex0) against Convex1
{
ConvexShape convexB = colB.CollisionShape as ConvexShape;
SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
colA.HitFraction = result.Fraction;
if (colB.HitFraction > result.Fraction)
colB.HitFraction = result.Fraction;
if (resultFraction > result.Fraction)
resultFraction = result.Fraction;
}
}
return resultFraction;
}
/// <summary>
/// cast a convex against another convex object
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
Matrix transformA = fromA;
Matrix transformB = fromB;
transformA.Translation = new Vector3(0, 0, 0);
transformB.Translation = new Vector3(0, 0, 0);
combined.TransformA = transformA;
combined.TransformB = transformB;
float radius = 0.01f;
float lambda = 0;
Vector3 s = rayFromLocalA.Translation;
Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
Vector3 x = s;
Vector3 n = new Vector3();
Vector3 c = new Vector3();
bool hasResult = false;
float lastLambda = lambda;
IConvexPenetrationDepthSolver penSolver = null;
Matrix identityTransform = Matrix.Identity;
SphereShape raySphere = new SphereShape(0.0f);
raySphere.Margin = 0.0f;
Matrix sphereTransform = Matrix.Identity;
sphereTransform.Translation = rayFromLocalA.Translation;
result.DrawCoordSystem(sphereTransform);
{
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
hasResult = pointCollector.HasResult;
c = pointCollector.PointInWorld;
n = pointCollector.NormalOnBInWorld;
}
if (hasResult)
{
float dist = (c - x).Length();
if (dist < radius)
{
lastLambda = 1.0f;
}
while (dist > radius)
{
n = x - c;
float dot = Vector3.Dot(n, r);
if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon))
{
return(false);
}
lambda = lambda - Vector3.Distance(n, n) / dot;
if (lambda <= lastLambda)
{
break;
}
lastLambda = lambda;
x = s + lambda * r;
sphereTransform.Translation = x;
result.DrawCoordSystem(sphereTransform);
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
c = pointCollector.PointInWorld;
dist = (c - x).Length();
}
else
{
return(false);
}
}
if (lastLambda < 1.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
}
return(false);
}
// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
// This allows more customization.
public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape=new SphereShape(0.0f);
if (collisionShape.IsConvex)
{
CastResult castResult = new CastResult();
castResult.Fraction = 1f;//??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
{
//add hit
if (castResult.Normal.LengthSquared() > 0.0001f)
{
castResult.Normal.Normalize();
if (castResult.Fraction < resultCallback.ClosestHitFraction)
{
CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
new LocalShapeInfo(),
castResult.Normal,
castResult.Fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
else
{
if (collisionShape.IsConcave)
{
TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.HitFraction = resultCallback.ClosestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
}
else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound)
{
CompoundShape compoundShape = collisionShape as CompoundShape;
for (int i = 0; i < compoundShape.ChildShapeCount; i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
}
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
_simplexSolver.Reset();
// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
float boundingRadiusA = _convexA.GetAngularMotionDisc();
float boundingRadiusB = _convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
angVelB.Length() * boundingRadiusB;
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1f, 0f, 0f);
int maxIter = MaxIterations;
Vector3 n = new Vector3();
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//float epsilon = 0.001f;
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin=0f;
//result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
gjk.setIgnoreMargin(true);
input.TransformA = fromA;
input.TransformB = fromB;
DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
hasResult = pointCollector1.HasResult;
c = pointCollector1.PointInWorld;
if (hasResult)
{
float dist;
dist = pointCollector1.Distance;
n = pointCollector1.NormalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
return false; //todo: report a failure
float dLambda = 0f;
//calculate safe moving fraction from distance / (linear+rotational velocity)
//float clippedDist = GEN_min(angularConservativeRadius,dist);
//float clippedDist = dist;
float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f) return false;
if (lambda < 0f) return false;
//todo: next check with relative epsilon
if (lambda <= lastLambda)
break;
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = interpolatedTransA;
input.TransformB = interpolatedTransB;
// !!!!!!!!!!
r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0f)
{
//degenerate ?!
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
c = pointCollector.PointInWorld;
dist = pointCollector.Distance;
}
else
{
//??
return false;
}
}
result.Fraction = lambda;
result.Normal = n;
return true;
}
return false;
}
public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex)
{
//do a swept sphere for now
Matrix ident = Matrix.Identity;
CastResult castResult = new CastResult();
castResult.Fraction = _hitFraction;
SphereShape pointShape = new SphereShape(_ccdSphereRadius);
TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
//local space?
if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,
ident, ident, castResult))
{
if (_hitFraction > castResult.Fraction)
_hitFraction = castResult.Fraction;
}
}
public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
{
this._sphere = sphere;
this._triangle = triangle;
}
/// <summary>
/// cast a convex against another convex object
/// </summary>
/// <param name="fromA"></param>
/// <param name="toA"></param>
/// <param name="fromB"></param>
/// <param name="toB"></param>
/// <param name="result"></param>
/// <returns></returns>
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
Matrix transformA = fromA;
Matrix transformB = fromB;
transformA.Translation = new Vector3(0, 0, 0);
transformB.Translation = new Vector3(0, 0, 0);
combined.TransformA = transformA;
combined.TransformB = transformB;
float radius = 0.01f;
float lambda = 0;
Vector3 s = rayFromLocalA.Translation;
Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
Vector3 x = s;
Vector3 n = new Vector3();
Vector3 c = new Vector3();
bool hasResult = false;
float lastLambda = lambda;
IConvexPenetrationDepthSolver penSolver = null;
Matrix identityTransform = Matrix.Identity;
SphereShape raySphere = new SphereShape(0.0f);
raySphere.Margin=0.0f;
Matrix sphereTransform = Matrix.Identity;
sphereTransform.Translation = rayFromLocalA.Translation;
result.DrawCoordSystem(sphereTransform);
{
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
hasResult = pointCollector.HasResult;
c = pointCollector.PointInWorld;
n = pointCollector.NormalOnBInWorld;
}
if (hasResult)
{
float dist = (c - x).Length();
if (dist < radius)
{
lastLambda = 1.0f;
}
while (dist > radius)
{
n = x - c;
float dot = Vector3.Dot(n, r);
if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
lambda = lambda - Vector3.Distance(n, n) / dot;
if (lambda <= lastLambda) break;
lastLambda = lambda;
x = s + lambda * r;
sphereTransform.Translation = x;
result.DrawCoordSystem(sphereTransform);
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = sphereTransform;
input.TransformB = identityTransform;
gjk.GetClosestPoints(input, pointCollector, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
c = pointCollector.PointInWorld;
dist = (c - x).Length();
}
else
{
return false;
}
}
if (lastLambda < 1.0f)
{
result.Fraction = lastLambda;
result.Normal = n;
return true;
}
}
return false;
}
/// <summary>
/// Reinitializes physics.
/// </summary>
public void ResetPhysics()
{
_collisionDispatcher = new CollisionDispatcher();
if (_useSweepAndPrune)
{
Vector3 worldAabbMin = new Vector3(-10000, -10000, -10000);
Vector3 worldAabbMax = new Vector3(10000, 10000, 10000);
const int maxProxies = 32766;
_broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
}
else
_broadphase = new SimpleBroadphase();
_solver = new SequentialImpulseConstraintSolver();
_world = new DiscreteDynamicsWorld(_collisionDispatcher, _broadphase, _solver);
//world.setConstraintSolver(solver);
_world.Gravity = new Vector3(0, -10, 0);
_shapePtr = new CollisionShape[4];
_shapePtr[0] = new BoxShape(new Vector3(50, 10, 50));
_shapePtr[1] = new CylinderShape(new Vector3(_cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin));
_shapePtr[2] = new SphereShape(_cubeHalfExtent);
_shapePtr[3] = new BoxShape(new Vector3(_cubeHalfExtent, _cubeHalfExtent, _cubeHalfExtent));
_shapeIndex = new int[_maxNumObjects];
Matrix tr = Matrix.Identity;
for (int i = 0; i < _numObjects; i++)
{
if (i > 0)
// set shape
_shapeIndex[i] = 1;
else
_shapeIndex[i] = 0;
}
GC.Collect();
}
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
_simplexSolver.Reset();
// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
float boundingRadiusA = _convexA.GetAngularMotionDisc();
float boundingRadiusB = _convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
angVelB.Length() * boundingRadiusB;
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1f, 0f, 0f);
int maxIter = MaxIterations;
Vector3 n = new Vector3();
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//float epsilon = 0.001f;
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin = 0f;
//result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
gjk.setIgnoreMargin(true);
input.TransformA = fromA;
input.TransformB = fromB;
DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
hasResult = pointCollector1.HasResult;
c = pointCollector1.PointInWorld;
if (hasResult)
{
float dist;
dist = pointCollector1.Distance;
n = pointCollector1.NormalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return(false); //todo: report a failure
}
float dLambda = 0f;
//calculate safe moving fraction from distance / (linear+rotational velocity)
//float clippedDist = GEN_min(angularConservativeRadius,dist);
//float clippedDist = dist;
float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f)
{
return(false);
}
if (lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
break;
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = interpolatedTransA;
input.TransformB = interpolatedTransB;
// !!!!!!!!!!
r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0f)
{
//degenerate ?!
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
c = pointCollector.PointInWorld;
dist = pointCollector.Distance;
}
else
{
//??
return(false);
}
}
result.Fraction = lambda;
result.Normal = n;
return(true);
}
return(false);
}
// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
// This allows more customization.
public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = new SphereShape(0.0f);
if (collisionShape.IsConvex)
{
CastResult castResult = new CastResult();
castResult.Fraction = 1f; //??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
{
//add hit
if (castResult.Normal.LengthSquared() > 0.0001f)
{
castResult.Normal.Normalize();
if (castResult.Fraction < resultCallback.ClosestHitFraction)
{
CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
(
collisionObject,
new LocalShapeInfo(),
castResult.Normal,
castResult.Fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
else
{
if (collisionShape.IsConcave)
{
TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
rcb.HitFraction = resultCallback.ClosestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
}
else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound)
{
CompoundShape compoundShape = collisionShape as CompoundShape;
for (int i = 0; i < compoundShape.ChildShapeCount; i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans, rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
}
public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
//Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
//col0->m_worldTransform,
float resultFraction = 1f;
float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
if (squareMotA < colA.CcdSquareMotionThreshold &&
squareMotB < colB.CcdSquareMotionThreshold)
{
return(resultFraction);
}
if (DisableCcd)
{
return(1f);
}
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
// Convex0 against sphere for Convex1
{
ConvexShape convexA = colA.CollisionShape as ConvexShape;
SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
//Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
{
colA.HitFraction = result.Fraction;
}
if (colB.HitFraction > result.Fraction)
{
colB.HitFraction = result.Fraction;
}
if (resultFraction > result.Fraction)
{
resultFraction = result.Fraction;
}
}
}
// Sphere (for convex0) against Convex1
{
ConvexShape convexB = colB.CollisionShape as ConvexShape;
SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
colB.WorldTransform, colB.InterpolationWorldTransform, result))
{
//store result.m_fraction in both bodies
if (colA.HitFraction > result.Fraction)
{
colA.HitFraction = result.Fraction;
}
if (colB.HitFraction > result.Fraction)
{
colB.HitFraction = result.Fraction;
}
if (resultFraction > result.Fraction)
{
resultFraction = result.Fraction;
}
}
}
return(resultFraction);
}