public virtual void ProcessTriangle(ObjectArray<Vector3> triangle, int partId, int triangleIndex)
{
TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
triangleShape.Margin = m_triangleCollisionMargin;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented ref code below
#if USE_SUBSIMPLEX_CONVEX_CAST
SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(m_convexShape, triangleShape, simplexSolver);
#else
//btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver);
ContinuousConvexCollision convexCaster = new ContinuousConvexCollision(m_convexShape,triangleShape,simplexSolver,gjkEpaPenetrationSolver);
#endif //#USE_SUBSIMPLEX_CONVEX_CAST
CastResult castResult = new CastResult();
castResult.m_fraction = 1f;
if (convexCaster.CalcTimeOfImpact(ref m_convexShapeFrom,ref m_convexShapeTo,ref m_triangleToWorld, ref m_triangleToWorld, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < m_hitFraction)
{
/* btContinuousConvexCast's normal is already in world space */
/*
#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
#endif //USE_SUBSIMPLEX_CONVEX_CAST
*/
castResult.m_normal.Normalize();
ReportHit (ref castResult.m_normal,ref castResult.m_hitPoint,castResult.m_fraction,partId,triangleIndex);
}
}
}
}
public void ProcessTriangle(ObjectArray<Vector3> triangle, int partId, int triangleIndex)
{
//do a swept sphere for now
Matrix ident = Matrix.Identity;
CastResult castResult = new CastResult();
castResult.m_fraction = m_hitFraction;
SphereShape pointShape = new SphereShape(m_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(ref m_ccdSphereFromTrans,ref m_ccdSphereToTrans,
ref ident,ref ident,castResult))
{
if (m_hitFraction > castResult.m_fraction)
{
m_hitFraction = castResult.m_fraction;
}
}
}
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,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
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform().Translation - body0.GetWorldTransform().Translation).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform().Translation - body1.GetWorldTransform().Translation).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return resultFraction;
}
//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 convex0 = (ConvexShape)(body0.GetCollisionShape());
SphereShape sphere1 = new SphereShape(body1.GetCcdSweptSphereRadius()); //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 ccd1 = new GjkConvexCast( convex0 ,sphere1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction()> result.m_fraction)
{
body0.SetHitFraction( result.m_fraction );
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = (ConvexShape)(body1.GetCollisionShape());
SphereShape sphere0 = new SphereShape(body0.GetCcdSweptSphereRadius()); //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 ccd1 = new GjkConvexCast(sphere0,convex1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction( result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
return resultFraction;
}
public virtual bool CalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = Vector3.Zero, angVelA = Vector3.Zero, linVelB = Vector3.Zero, angVelB = Vector3.Zero;
TransformUtil.CalculateVelocity(ref fromA,ref toA,1f,ref linVelA,ref angVelA);
TransformUtil.CalculateVelocity(ref fromB,ref toB,1f,ref linVelB,ref angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
Vector3 relLinVel = (linVelB-linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return false;
}
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1,0,0);
int maxIter = MAX_ITERATIONS;
Vector3 n = Vector3.Zero;
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
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(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(input,pointCollector1,null,false);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
Vector3 colour = new Vector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
float dLambda = 0f;
projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//calculate safe moving fraction from distance / (linear+rotational velocity)
//btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
//btScalar clippedDist = dist;
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=MathUtil.SIMD_EPSILON)
{
return false;
}
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f || lambda < 0f)
{
return false;
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = Matrix.Identity, interpolatedTransB = Matrix.Identity, relativeTrans = Matrix.Identity;
TransformUtil.IntegrateTransform(ref fromA,ref linVelA,ref angVelA,lambda,ref interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB,ref linVelB,ref angVelB,lambda,ref interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = MathUtil.InverseTimes(ref interpolatedTransB, ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA.Translation, 0.2f, new Vector3(1, 0, 0));
}
result.DebugDraw( lambda );
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
input.m_transformA = interpolatedTransA;
input.m_transformB = interpolatedTransB;
gjk.GetClosestPoints(input,pointCollector,null,false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
//degenerate ?!
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;//.setValue(1,1,1);// = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= result.m_allowedPenetration)//SIMD_EPSILON)
{
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
/*
//todo:
//if movement away from normal, discard result
btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
if (result.m_fraction < btScalar(1.))
{
if (move.dot(result.m_normal) <= btScalar(0.))
{
}
}
*/
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape,ref Matrix convexFromTrans,ref Matrix convexToTrans,
CollisionObject collisionObject,CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
//BT_PROFILE("convexSweepConvex");
CastResult castResult = new CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = (ConvexShape) collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape,convexShape,simplexSolver, gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
IConvexCast castPtr = convexCaster1;
if (castPtr.CalcTimeOfImpact(ref convexFromTrans,ref convexToTrans,ref colObjWorldTransform,ref colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, ref convexFromTrans,ref convexToTrans,resultCallback,collisionObject,triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
triangleMesh.PerformConvexCast(tccb,ref convexFromLocal,ref convexToLocal,ref boxMinLocal, ref boxMaxLocal);
}
else
{
//BT_PROFILE("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);
BridgeTriangleConvexcastCallback2 tccb = new BridgeTriangleConvexcastCallback2(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 boxMinLocal = new Vector3();
Vector3 boxMaxLocal = new Vector3();
castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
Vector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal,ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
Vector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal,ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
//BT_PROFILE("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans);
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);
LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb, allowedPenetration);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
}
}
}
}
}
public virtual void RayTestSingle(ref Matrix rayFromTrans,ref Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = new SphereShape(0.0f);
pointShape.Margin = 0f;
ConvexShape castShape = pointShape;
if (collisionShape.IsConvex())
{
// BT_PROFILE("rayTestConvex");
CastResult castResult = new CastResult();
castResult.m_fraction = resultCallback.m_closestHitFraction;
ConvexShape convexShape = (ConvexShape)collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
// FIXME - MAN - convexcat here seems to make big difference to forklift.
SubSimplexConvexCast convexCaster = new SubSimplexConvexCast(castShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster = new GjkConvexCast(castShape, convexShape, simplexSolver);
//#else
//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
//#endif //#USE_SUBSIMPLEX_CONVEX_CAST
if (convexCaster.CalcTimeOfImpact(ref rayFromTrans,ref rayToTrans,ref colObjWorldTransform,ref colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
//if (resultCallback.m_closestHitFraction != 1f)
//{
// int ibreak = 0;
// convexCaster.calcTimeOfImpact(ref rayFromTrans, ref rayToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult);
//}
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = Vector3.TransformNormal(castResult.m_normal,rayFromTrans);
//#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.Normalize();
LocalRayResult localRayResult = new LocalRayResult(
collisionObject,
null,
ref castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localRayResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
if (collisionShape.IsConcave())
{
// BT_PROFILE("rayTestConcave");
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape)
{
///optimized version for btBvhTriangleMeshShape
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);
Matrix transform = Matrix.Identity;
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(ref rayFromLocal,ref rayToLocal, resultCallback,collisionObject,triangleMesh,ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.PerformRaycast(rcb,ref rayFromLocal,ref rayToLocal);
rcb.Cleanup();
}
else
{
//generic (slower) case
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);
//ConvexCast::CastResult
Matrix transform = Matrix.Identity;
BridgeTriangleConcaveRaycastCallback rcb = new BridgeTriangleConcaveRaycastCallback(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape,ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathUtil.VectorMin(ref rayToLocal,ref rayAabbMinLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathUtil.VectorMax(ref rayToLocal,ref rayAabbMaxLocal);
concaveShape.ProcessAllTriangles(rcb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
rcb.Cleanup();
}
}
else
{
// BT_PROFILE("rayTestCompound");
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
if (collisionShape.IsCompound())
{
CompoundShape compoundShape = (CompoundShape)(collisionShape);
int i=0;
for (i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans) ;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);
LocalInfoAdder2 my_cb = new LocalInfoAdder2(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
RayTestSingle(ref rayFromTrans,ref rayToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
my_cb.cleanup();
}
}
}
}
}
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
public virtual bool CalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
m_simplexSolver.Reset();
Vector3 linVelA = toA.Translation-fromA.Translation;
Vector3 linVelB = toB.Translation-fromB.Translation;
float lambda = 0f;
Matrix interpolatedTransA = fromA;
Matrix interpolatedTransB = fromB;
///take relative motion
Vector3 r = (linVelA-linVelB);
Vector3 v;
Vector3 supportTemp = MathUtil.TransposeTransformNormal(-r, fromA);
Vector3 supVertexA = Vector3.Transform(m_convexA.LocalGetSupportingVertex(ref supportTemp),fromA);
supportTemp = MathUtil.TransposeTransformNormal(r, fromB);
Vector3 foo = m_convexB.LocalGetSupportingVertex(ref supportTemp);
Vector3 supVertexB = Vector3.Transform(foo, fromB);
v = supVertexA-supVertexB;
int maxIter = MAX_ITERATIONS;
Vector3 n = Vector3.Zero;
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
Vector3 w,p;
float VdotR;
while ( (dist2 > epsilon) && (maxIter-- > 0))
{
supportTemp = MathUtil.TransposeTransformNormal(-v, interpolatedTransA);
supVertexA = Vector3.Transform(m_convexA.LocalGetSupportingVertex(ref supportTemp), interpolatedTransA);
supportTemp = MathUtil.TransposeTransformNormal(v, interpolatedTransB);
supVertexB = Vector3.Transform(m_convexB.LocalGetSupportingVertex(ref supportTemp), interpolatedTransB);
w = supVertexA-supVertexB;
float VdotW = Vector3.Dot(v,w);
if (lambda > 1.0f)
{
return false;
}
if ( VdotW > 0f)
{
VdotR = Vector3.Dot(v,r);
if (VdotR >= -(MathUtil.SIMD_EPSILON*MathUtil.SIMD_EPSILON))
{
return false;
}
else
{
lambda = lambda - VdotW / VdotR;
//interpolate to next lambda
// x = s + lambda * r;
interpolatedTransA.Translation = MathUtil.Interpolate3(fromA.Translation,toA.Translation,lambda);
interpolatedTransB.Translation = MathUtil.Interpolate3(fromB.Translation, toB.Translation, lambda);
//m_simplexSolver.reset();
//check next line
w = supVertexA-supVertexB;
lastLambda = lambda;
n = v;
hasResult = true;
}
}
///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
if (!m_simplexSolver.InSimplex(ref w))
{
m_simplexSolver.AddVertex(ref w, ref supVertexA, ref supVertexB);
}
if (m_simplexSolver.Closest(ref v))
{
dist2 = v.LengthSquared();
hasResult = true;
//todo: check this normal for validity
//n=v;
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
//printf("DIST2=%f\n",dist2);
//printf("numverts = %i\n",m_simplexSolver.numVertices());
} else
{
dist2 = 0f;
}
}
//int numiter = MAX_ITERATIONS - maxIter;
// printf("number of iterations: %d", numiter);
//don't report a time of impact when moving 'away' from the hitnormal
result.m_fraction = lambda;
if (n.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
{
result.m_normal = Vector3.Normalize(n);
}
else
{
result.m_normal = Vector3.Zero;
}
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (Vector3.Dot(result.m_normal, r) >= -result.m_allowedPenetration)
{
return false;
}
Vector3 hitA = Vector3.Zero, hitB = Vector3.Zero;
m_simplexSolver.ComputePoints(ref hitA,ref hitB);
result.m_hitPoint=hitB;
return true;
}
public virtual bool russianCalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
MinkowskiSumShape convex = new MinkowskiSumShape(m_convexA, m_convexB);
Matrix rayFromLocalA;
Matrix rayToLocalA;
rayFromLocalA = Matrix.Invert(fromA) * fromB;
rayToLocalA = Matrix.Invert(toA) * toB;
m_simplexSolver.Reset();
//convex.TransformB = rayFromLocalA;
Matrix temp = Matrix.CreateFromQuaternion(Quaternion.CreateFromRotationMatrix(rayFromLocalA));
convex.SetTransformB(ref temp);
float lambda = 0;
//todo: need to verify this:
//because of minkowski difference, we need the inverse direction
Vector3 s = -rayFromLocalA.Translation;
Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
Vector3 x = s;
Vector3 v;
Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(ref r);
v = x - arbitraryPoint;
int maxIter = MAX_ITERATIONS;
Vector3 n = new Vector3();
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
Vector3 w = Vector3.Zero, p = Vector3.Zero;
float VdotR;
while ((dist2 > epsilon) && (maxIter-- != 0))
{
p = convex.LocalGetSupportingVertex(ref v);
w = x - p;
float VdotW = Vector3.Dot(v, w);
if (VdotW > 0)
{
VdotR = Vector3.Dot(v, r);
if (VdotR >= -(MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
return false;
else
{
lambda = lambda - VdotW / VdotR;
x = s + lambda * r;
m_simplexSolver.Reset();
//check next line
w = x - p;
lastLambda = lambda;
n = v;
}
}
m_simplexSolver.AddVertex(ref w, ref x, ref p);
if (m_simplexSolver.Closest(ref v))
{
dist2 = v.LengthSquared();
}
else
{
dist2 = 0f;
}
}
result.m_fraction = lambda;
result.m_normal = n;
return true;
}
