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); }
public RayTester(CollisionObject collisionObject, CompoundShape compoundShape, ref IndexedMatrix colObjWorldTransform, ref IndexedMatrix rayFromTrans, ref IndexedMatrix rayToTrans, RayResultCallback resultCallback) { m_collisionObject = collisionObject; m_compoundShape = compoundShape; m_colObjWorldTransform = colObjWorldTransform; m_rayFromTrans = rayFromTrans; m_rayToTrans = rayToTrans; m_resultCallback = resultCallback; }
/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. public virtual void RayTest(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, RayResultCallback resultCallback) { BulletGlobals.StartProfile("rayTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test using (SingleRayCallback rayCB = BulletGlobals.SingleRayCallbackPool.Get()) { rayCB.Initialize(ref rayFromWorld, ref rayToWorld, this, resultCallback); #if !USE_BRUTEFORCE_RAYBROADPHASE m_broadphasePairCache.RayTest(ref rayFromWorld, ref rayToWorld, rayCB); #else for (int i=0;i<GetNumCollisionObjects();i++) { rayCB.Process(m_collisionObjects[i].GetBroadphaseHandle()); } #endif //USE_BRUTEFORCE_RAYBROADPHASE } BulletGlobals.StopProfile(); }
public void Initialize(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, CollisionWorld world, RayResultCallback resultCallback) { m_rayFromWorld = rayFromWorld; m_rayToWorld = rayToWorld; m_world = world; m_resultCallback = resultCallback; m_rayFromTrans = IndexedMatrix.CreateTranslation(m_rayFromWorld); m_rayToTrans = IndexedMatrix.CreateTranslation(m_rayToWorld); IndexedVector3 rayDir = (rayToWorld - rayFromWorld); rayDir.Normalize(); ///what about division by zero? -. just set rayDirection[i] to INF/1e30 m_rayDirectionInverse.X = MathUtil.FuzzyZero(rayDir.X) ? float.MaxValue : 1f / rayDir.X; m_rayDirectionInverse.Y = MathUtil.FuzzyZero(rayDir.Y) ? float.MaxValue : 1f / rayDir.Y; m_rayDirectionInverse.Z = MathUtil.FuzzyZero(rayDir.Z) ? float.MaxValue : 1f / rayDir.Z; m_signs[0] = m_rayDirectionInverse.X < 0.0f; m_signs[1] = m_rayDirectionInverse.Y < 0.0f; m_signs[2] = m_rayDirectionInverse.Z < 0.0f; m_lambda_max = rayDir.Dot(m_rayToWorld - m_rayFromWorld); }
public LocalInfoAdder2(int i, RayResultCallback user) { m_i = i; m_userCallback = user; m_closestHitFraction = user.m_closestHitFraction; }
public SingleRayCallback(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, CollisionWorld world, RayResultCallback resultCallback) { Initialize(ref rayFromWorld,ref rayToWorld,world,resultCallback); }
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 RayTest(ref IndexedVector3 rayFromWorld, ref IndexedVector3 rayToWorld, RayResultCallback resultCallback) { int ibreak = 0; }
//!@} //! virtual method for ray collision public virtual void RayTest(ref IndexedVector3 rayFrom, ref IndexedVector3 rayTo, RayResultCallback resultCallback) { }