public override float AddSingleResult(LocalConvexResult convexResult,bool normalInWorldSpace)
{
//caller already does the filter on the m_closestHitFraction
//btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
m_closestHitFraction = convexResult.m_hitFraction;
m_hitCollisionObject = convexResult.m_hitCollisionObject;
if (normalInWorldSpace)
{
m_hitNormalWorld = convexResult.m_hitNormalLocal;
} else
{
///need to transform normal into worldspace
m_hitNormalWorld = Vector3.TransformNormal(convexResult.m_hitNormalLocal,m_hitCollisionObject.GetWorldTransform());
}
m_hitPointWorld = convexResult.m_hitPointLocal;
return convexResult.m_hitFraction;
}
public override float ReportHit(ref Vector3 hitNormalLocal, ref Vector3 hitPointLocal, float hitFraction, int partId, int triangleIndex )
{
LocalShapeInfo shapeInfo = new LocalShapeInfo();
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
if (hitFraction <= m_resultCallback.m_closestHitFraction)
{
LocalConvexResult convexResult = new LocalConvexResult
(m_collisionObject,
shapeInfo,
ref hitNormalLocal,
ref hitPointLocal,
hitFraction);
bool normalInWorldSpace = true;
return m_resultCallback.AddSingleResult(convexResult,normalInWorldSpace);
}
return hitFraction;
}
public override float AddSingleResult(LocalConvexResult convexResult, bool normalInWorldSpace)
{
if (convexResult.m_hitCollisionObject == m_me)
return 1.0f;
//ignore result if there is no contact response
if (!convexResult.m_hitCollisionObject.HasContactResponse())
return 1.0f;
Vector3 linVelA, linVelB;
linVelA = m_convexToWorld - m_convexFromWorld;
linVelB = Vector3.Zero;//toB.getOrigin()-fromB.getOrigin();
Vector3 relativeVelocity = (linVelA - linVelB);
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (Vector3.Dot(convexResult.m_hitNormalLocal, relativeVelocity) >= -m_allowedPenetration)
return 1f;
return base.AddSingleResult(convexResult, normalInWorldSpace);
}
public abstract float AddSingleResult(LocalConvexResult convexResult,bool normalInWorldSpace);
/// 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);
}
}
}
}
}