public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration) { IndexedMatrix convexFromTrans = convexFromWorld; IndexedMatrix convexToTrans = convexToWorld; IndexedVector3 castShapeAabbMin; IndexedVector3 castShapeAabbMax; /* Compute AABB that encompasses angular movement */ IndexedVector3 linVel, angVel; TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel); // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale? IndexedMatrix R = IndexedMatrix.Identity; R.SetRotation(convexFromTrans.GetRotation()); castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax); /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) for (int i = 0; i < m_overlappingObjects.Count; i++) { CollisionObject collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); IndexedVector3 collisionObjectAabbMin; IndexedVector3 collisionObjectAabbMax; IndexedMatrix t = collisionObject.GetWorldTransform(); collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax); AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax); float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing IndexedVector3 hitNormal; if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal)) { IndexedMatrix wt = collisionObject.GetWorldTransform(); CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans, collisionObject, collisionObject.GetCollisionShape(), ref wt, resultCallback, allowedCcdPenetration); } } } }