public static void Initialize(ConvexShape shape0,ref IndexedMatrix wtrs0, ConvexShape shape1,ref IndexedMatrix wtrs1, ref GjkEpaSolver2Results results, GjkEpaSolver2MinkowskiDiff shapeR, bool withmargins) { /* Results */ results.witnesses0 = IndexedVector3.Zero; results.witnesses1 = IndexedVector3.Zero; results.status = GjkEpaSolver2Status.Separated; /* Shape */ shapeR.m_shapes[0] = shape0; shapeR.m_shapes[1] = shape1; shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis); shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1); } #endif shapeR.EnableMargin(withmargins); }
public static void RayTestSingle(ref IndexedMatrix rayFromTrans, ref IndexedMatrix rayToTrans, CollisionObject collisionObject, CollisionShape collisionShape, ref IndexedMatrix colObjWorldTransform, RayResultCallback resultCallback) { SphereShape pointShape = BulletGlobals.SphereShapePool.Get(); pointShape.Initialize(0.0f); pointShape.SetMargin(0f); ConvexShape castShape = pointShape; if (collisionShape.IsConvex()) { BulletGlobals.StartProfile("rayTestConvex"); CastResult castResult = BulletGlobals.CastResultPool.Get(); castResult.m_fraction = resultCallback.m_closestHitFraction; ConvexShape convexShape = collisionShape as ConvexShape; VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get(); //#define USE_SUBSIMPLEX_CONVEX_CAST 1 //#ifdef USE_SUBSIMPLEX_CONVEX_CAST // FIXME - MAN - convexcat here seems to make big difference to forklift. SubSimplexConvexCast convexCaster = BulletGlobals.SubSimplexConvexCastPool.Get(); convexCaster.Initialize(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 = rayFromTrans._basis * castResult.m_normal; //#endif //USE_SUBSIMPLEX_CONVEX_CAST castResult.m_normal.Normalize(); LocalRayResult localRayResult = new LocalRayResult( collisionObject, //null, // updated to allow different ctor on struct ref castResult.m_normal, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.AddSingleResult(ref localRayResult, normalInWorldSpace); } } } castResult.Cleanup(); BulletGlobals.SubSimplexConvexCastPool.Free(convexCaster); BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver); BulletGlobals.StopProfile(); } else { if (collisionShape.IsConcave()) { BulletGlobals.StartProfile("rayTestConcave"); if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape) { ///optimized version for btBvhTriangleMeshShape BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape; IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse(); IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin; IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin; IndexedMatrix transform = IndexedMatrix.Identity; using (BridgeTriangleRaycastCallback rcb = BulletGlobals.BridgeTriangleRaycastCallbackPool.Get()) { rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, triangleMesh, ref transform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh.PerformRaycast(rcb, ref rayFromLocal, ref rayToLocal); } } else if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE && collisionShape is HeightfieldTerrainShape) { ///optimized version for btBvhTriangleMeshShape HeightfieldTerrainShape heightField = (HeightfieldTerrainShape)collisionShape; IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse(); IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin; IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin; IndexedMatrix transform = IndexedMatrix.Identity; using (BridgeTriangleConcaveRaycastCallback rcb = BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.Get()) { rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, heightField, ref transform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; heightField.PerformRaycast(rcb, ref rayFromLocal, ref rayToLocal); } } else { //generic (slower) case ConcaveShape concaveShape = (ConcaveShape)collisionShape; IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse(); IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin; IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin; //ConvexCast::CastResult IndexedMatrix transform = IndexedMatrix.Identity; using (BridgeTriangleConcaveRaycastCallback rcb = BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.Get()) { rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape, ref transform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; IndexedVector3 rayAabbMinLocal = rayFromLocal; MathUtil.VectorMin(ref rayToLocal, ref rayAabbMinLocal); IndexedVector3 rayAabbMaxLocal = rayFromLocal; MathUtil.VectorMax(ref rayToLocal, ref rayAabbMaxLocal); concaveShape.ProcessAllTriangles(rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal); } } BulletGlobals.StopProfile(); } else { BulletGlobals.StartProfile("rayTestCompound"); ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt if (collisionShape.IsCompound()) { CompoundShape compoundShape = collisionShape as CompoundShape; Dbvt dbvt = compoundShape.GetDynamicAabbTree(); RayTester rayCB = new RayTester( collisionObject, compoundShape, ref colObjWorldTransform, ref rayFromTrans, ref rayToTrans, resultCallback); #if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION if (dbvt != null) { IndexedVector3 localRayFrom = colObjWorldTransform.InverseTimes(ref rayFromTrans)._origin; IndexedVector3 localRayTo = colObjWorldTransform.InverseTimes(ref rayToTrans)._origin; Dbvt.RayTest(dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB); } else #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION { for (int i = 0, n = compoundShape.GetNumChildShapes(); i < n; ++i) { rayCB.Process(i); } } rayCB.Cleanup(); BulletGlobals.StopProfile(); } } } BulletGlobals.SphereShapePool.Free(pointShape); }