public void GImpactVsConcave(
CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
ConcaveShape shape1, bool swapped)
{
GImpactTriangleCallback tricallback = new GImpactTriangleCallback();
tricallback.algorithm = this;
tricallback.body0 = body0;
tricallback.body1 = body1;
tricallback.gimpactshape0 = shape0;
tricallback.swapped = swapped;
tricallback.margin = shape1.GetMargin();
//getting the trimesh AABB
IndexedMatrix gimpactInConcaveSpace;
gimpactInConcaveSpace = body1.GetWorldTransform().Inverse() * body0.GetWorldTransform();
IndexedVector3 minAABB, maxAABB;
shape0.GetAabb(gimpactInConcaveSpace, out minAABB, out maxAABB);
shape1.ProcessAllTriangles(tricallback, ref minAABB, ref maxAABB);
}
//public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
//{
// CollisionObject convexBody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// if (triBody.getCollisionShape().isConcave())
// {
// CollisionObject triOb = triBody;
// ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape());
// if (convexBody.getCollisionShape().isConvex())
// {
// float collisionMarginTriangle = concaveShape.getMargin();
// resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
// //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
// //m_dispatcher.clearManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody);
// IndexedVector3 min = m_convexTriangleCallback.getAabbMin();
// IndexedVector3 max = m_convexTriangleCallback.getAabbMax();
// concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max );
// resultOut.refreshContactPoints();
// }
// }
//}
//public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
//{
// CollisionObject convexbody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
// //only perform CCD above a certain threshold, this prevents blocking on the long run
// //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
// float squareMot0 = (convexbody.getInterpolationWorldTransform()._origin - convexbody.getWorldTransform()._origin).LengthSquared();
// if (squareMot0 < convexbody.getCcdSquareMotionThreshold())
// {
// return 1f;
// }
// //const IndexedVector3& from = convexbody.m_worldTransform._origin;
// //IndexedVector3 to = convexbody.m_interpolationWorldTransform._origin;
// //todo: only do if the motion exceeds the 'radius'
// //IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// //IndexedMatrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform());
// //IndexedMatrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform());
// IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// IndexedMatrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform());
// IndexedMatrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform());
// if (triBody.getCollisionShape().isConcave())
// {
// IndexedVector3 rayAabbMin = convexFromLocal._origin;
// MathUtil.vectorMin(convexToLocal._origin, ref rayAabbMin);
// IndexedVector3 rayAabbMax = convexFromLocal._origin;
// MathUtil.vectorMax(convexToLocal._origin,ref rayAabbMax);
// float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
// rayAabbMin -= new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// rayAabbMax += new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// float curHitFraction = 1.0f; //is this available?
// LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
// convexbody.getCcdSweptSphereRadius(),curHitFraction);
// raycastCallback.m_hitFraction = convexbody.getHitFraction();
// CollisionObject concavebody = triBody;
// ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape();
// if (triangleMesh != null)
// {
// triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax);
// }
// if (raycastCallback.m_hitFraction < convexbody.getHitFraction())
// {
// convexbody.setHitFraction( raycastCallback.m_hitFraction);
// float result = raycastCallback.m_hitFraction;
// raycastCallback.cleanup();
// return result;
// }
// raycastCallback.cleanup();
// }
// return 1f;
//}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//fixme
CollisionObject convexBody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
if (triBody.GetCollisionShape().IsConcave())
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.GetCollisionShape() as ConcaveShape;
if (convexBody.GetCollisionShape().IsConvex())
{
float collisionMarginTriangle = concaveShape.GetMargin();
resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody);
IndexedVector3 min = m_convexTriangleCallback.GetAabbMin();
IndexedVector3 max = m_convexTriangleCallback.GetAabbMax();
concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min, ref max);
resultOut.RefreshContactPoints();
}
}
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.GetInterpolationWorldTransform()._origin - convexbody.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
{
return(1);
}
//IndexedMatrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
IndexedMatrix triInv = triBody.GetWorldTransform().Inverse();
IndexedMatrix convexFromLocal = triInv * convexbody.GetWorldTransform();
IndexedMatrix convexToLocal = triInv * convexbody.GetInterpolationWorldTransform();
if (triBody.GetCollisionShape().IsConcave())
{
IndexedVector3 rayAabbMin = convexFromLocal._origin;
MathUtil.VectorMin(convexToLocal._origin, ref rayAabbMin);
IndexedVector3 rayAabbMax = convexFromLocal._origin;
MathUtil.VectorMax(convexToLocal._origin, ref rayAabbMax);
IndexedVector3 ccdRadius0 = new IndexedVector3(convexbody.GetCcdSweptSphereRadius());
rayAabbMin -= ccdRadius0;
rayAabbMax += ccdRadius0;
float curHitFraction = 1f; //is this available?
using (LocalTriangleSphereCastCallback raycastCallback = BulletGlobals.LocalTriangleSphereCastCallbackPool.Get())
{
raycastCallback.Initialize(ref convexFromLocal, ref convexToLocal,
convexbody.GetCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody.GetHitFraction();
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
{
convexbody.SetHitFraction(raycastCallback.m_hitFraction);
return(raycastCallback.m_hitFraction);
}
}
}
return(1);
}
public void Initialize(ConvexShape castShape, ref IndexedMatrix from, ref IndexedMatrix to,
ConvexResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix triangleToWorld)
{
base.Initialize(castShape, ref from, ref to, ref triangleToWorld, triangleMesh.GetMargin());
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
}
public BridgeTriangleConvexcastCallback() { } // for pool
public BridgeTriangleConvexcastCallback(ConvexShape castShape, ref IndexedMatrix from, ref IndexedMatrix to,
ConvexResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix triangleToWorld) :
base(castShape, ref from, ref to, ref triangleToWorld, triangleMesh.GetMargin())
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
}
public void Initialize(ref IndexedVector3 from, ref IndexedVector3 to,
RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix colObjWorldTransform)
{
base.Initialize(ref from, ref to, resultCallback.m_flags);
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
m_colObjWorldTransform = colObjWorldTransform;
}
public BridgeTriangleConcaveRaycastCallback() { } // for pool
public BridgeTriangleConcaveRaycastCallback(ref IndexedVector3 from, ref IndexedVector3 to,
RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix colObjWorldTransform) :
//@BP Mod
base(ref from, ref to, resultCallback.m_flags)
{
m_resultCallback = resultCallback;
m_collisionObject = collisionObject;
m_triangleMesh = triangleMesh;
m_colObjWorldTransform = colObjWorldTransform;
}
public void GImpactVsConcave(
CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
ConcaveShape shape1, bool swapped)
{
GImpactTriangleCallback tricallback = new GImpactTriangleCallback();
tricallback.algorithm = this;
tricallback.body0 = body0;
tricallback.body1 = body1;
tricallback.gimpactshape0 = shape0;
tricallback.swapped = swapped;
tricallback.margin = shape1.GetMargin();
//getting the trimesh AABB
IndexedMatrix gimpactInConcaveSpace;
gimpactInConcaveSpace = body1.GetWorldTransform().Inverse() * body0.GetWorldTransform();
IndexedVector3 minAABB, maxAABB;
shape0.GetAabb(gimpactInConcaveSpace, out minAABB, out maxAABB);
shape1.ProcessAllTriangles(tricallback, ref minAABB, ref maxAABB);
}
public void GImpactVsShape(CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
CollisionShape shape1, bool swapped)
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape");
}
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
{
GImpactMeshShape meshshape0 = shape0 as GImpactMeshShape;
// check this...
//int& part = swapped ? m_part1 : m_part0;
//part = meshshape0.GetMeshPartCount();
int part = meshshape0.GetMeshPartCount();
while (part-- != 0)
{
GImpactVsShape(body0,
body1,
meshshape0.GetMeshPart(part),
shape1, swapped);
}
if (swapped)
{
m_part1 = part;
}
else
{
m_part0 = part;
}
return;
}
#if GIMPACT_VS_PLANE_COLLISION
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART &&
shape1.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
GImpactMeshShapePart shapepart = shape0 as GImpactMeshShapePart;
StaticPlaneShape planeshape = shape1 as StaticPlaneShape;
GImpactTrimeshpartVsPlaneCollision(body0, body1, shapepart, planeshape, swapped);
return;
}
#endif
if (shape1.IsCompound())
{
CompoundShape compoundshape = shape1 as CompoundShape;
GImpactVsCompoundshape(body0, body1, shape0, compoundshape, swapped);
return;
}
else if (shape1.IsConcave())
{
ConcaveShape concaveshape = shape1 as ConcaveShape;
GImpactVsConcave(body0, body1, shape0, concaveshape, swapped);
return;
}
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
ObjectArray <int> collided_results = new ObjectArray <int>(64);
GImpactVsShapeFindPairs(ref orgtrans0, ref orgtrans1, shape0, shape1, collided_results);
if (collided_results.Count == 0)
{
return;
}
shape0.LockChildShapes();
using (GIM_ShapeRetriever retriever0 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
{
retriever0.Initialize(shape0);
bool child_has_transform0 = shape0.ChildrenHasTransform();
int i = collided_results.Count;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape [{0}]", collided_results.Count);
}
while (i-- != 0)
{
int child_index = collided_results[i];
if (swapped)
{
m_triface1 = child_index;
}
else
{
m_triface0 = child_index;
}
CollisionShape colshape0 = retriever0.GetChildShape(child_index);
if (child_has_transform0)
{
body0.SetWorldTransform(orgtrans0 * shape0.GetChildTransform(child_index));
}
//collide two shapes
if (swapped)
{
ShapeVsShapeCollision(body1, body0, shape1, colshape0);
}
else
{
ShapeVsShapeCollision(body0, body1, colshape0, shape1);
}
//restore transforms
if (child_has_transform0)
{
body0.SetWorldTransform(ref orgtrans0);
}
}
shape0.UnlockChildShapes();
}
}