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 override float ReportHit(ref IndexedVector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) { LocalShapeInfo shapeInfo = new LocalShapeInfo(); shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; IndexedVector3 hitNormalWorld = m_colObjWorldTransform._basis * hitNormalLocal; LocalRayResult rayResult = new LocalRayResult (m_collisionObject, ref shapeInfo, ref hitNormalWorld, hitFraction); bool normalInWorldSpace = true; return m_resultCallback.AddSingleResult(ref rayResult, normalInWorldSpace); }
public override float AddSingleResult(ref LocalRayResult r, bool b) { LocalShapeInfo shapeInfo = new LocalShapeInfo(); shapeInfo.m_shapePart = -1; shapeInfo.m_triangleIndex = m_i; r.m_localShapeInfo = shapeInfo; //if (r.m_localShapeInfo == null) //{ // r.m_localShapeInfo = shapeInfo; //} float result = m_userCallback.AddSingleResult(ref r, b); m_closestHitFraction = m_userCallback.m_closestHitFraction; return result; }
public override float AddSingleResult(ref LocalRayResult rayResult,bool normalInWorldSpace) { m_collisionObject = rayResult.m_collisionObject; m_collisionObjects.Add(rayResult.m_collisionObject); IndexedVector3 hitNormalWorld; if (normalInWorldSpace) { hitNormalWorld = rayResult.m_hitNormalLocal; } else { ///need to transform normal into worldspace hitNormalWorld = m_collisionObject.GetWorldTransform()._basis * rayResult.m_hitNormalLocal; } m_hitNormalWorld.Add(hitNormalWorld); IndexedVector3 hitPointWorld = MathUtil.Interpolate3(ref m_rayFromWorld,ref m_rayToWorld,rayResult.m_hitFraction); m_hitPointWorld.Add(hitPointWorld); m_hitFractions.Add(rayResult.m_hitFraction); return m_closestHitFraction; }
//public void Initialize(Vector3 rayFromWorld, Vector3 rayToWorld) //{ // m_rayFromWorld = rayFromWorld; // m_rayToWorld = rayToWorld; //} public override float AddSingleResult(ref LocalRayResult rayResult, bool normalInWorldSpace) { //caller already does the filter on the m_closestHitFraction //btAssert(rayResult.m_hitFraction <= m_closestHitFraction); m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; uint obj = 0; object specialobj = m_collisionObject.GetUserPointer(); if (specialobj is uint) { obj = (uint) specialobj; if (obj == ObjectToAvoid) return 1.0f; } return base.AddSingleResult(ref rayResult, normalInWorldSpace); }
public override float AddSingleResult(ref LocalRayResult rayResult, bool normalInWorldSpace) { //caller already does the filter on the m_closestHitFraction //btAssert(rayResult.m_hitFraction <= m_closestHitFraction); m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; if (normalInWorldSpace) { m_hitNormalWorld = rayResult.m_hitNormalLocal; } else { ///need to transform normal into worldspace m_hitNormalWorld = m_collisionObject.GetWorldTransform()._basis * rayResult.m_hitNormalLocal; } m_hitPointWorld = MathUtil.Interpolate3(ref m_rayFromWorld, ref m_rayToWorld, rayResult.m_hitFraction); return rayResult.m_hitFraction; }
public abstract float AddSingleResult(ref LocalRayResult rayResult, bool normalInWorldSpace);
public override float AddSingleResult(ref LocalRayResult rayResult, bool normalInWorldSpace) { if (rayResult.m_collisionObject == m_me) return 1.0f; return base.AddSingleResult(ref rayResult, normalInWorldSpace); }