예제 #1
0
    /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
    public static void	ObjectQuerySingle(ConvexShape castShape,ref Matrix convexFromTrans,ref Matrix convexToTrans,
					  CollisionObject collisionObject,CollisionShape collisionShape,
					  ref Matrix colObjWorldTransform,
					  ConvexResultCallback resultCallback, float allowedPenetration)
    {
	    if (collisionShape.IsConvex())
	    {
		    //BT_PROFILE("convexSweepConvex");
		    CastResult castResult = new CastResult();
		    castResult.m_allowedPenetration = allowedPenetration;
		    castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??

		    ConvexShape convexShape = (ConvexShape) collisionShape;
		    VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
		    GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();
    		
		    ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape,convexShape,simplexSolver, gjkEpaPenetrationSolver);
		    //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
		    //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);

		    IConvexCast castPtr = convexCaster1;
		
		    if (castPtr.CalcTimeOfImpact(ref convexFromTrans,ref convexToTrans,ref colObjWorldTransform,ref colObjWorldTransform,castResult))
		    {
			    //add hit
			    if (castResult.m_normal.LengthSquared() > 0.0001f)
			    {
				    if (castResult.m_fraction < resultCallback.m_closestHitFraction)
				    {
					    castResult.m_normal.Normalize();
					    LocalConvexResult localConvexResult = new LocalConvexResult
								    (
									    collisionObject,
									    null,
									    ref castResult.m_normal,
									    ref castResult.m_hitPoint,
									    castResult.m_fraction
								    );

					    bool normalInWorldSpace = true;
					    resultCallback.AddSingleResult(localConvexResult, normalInWorldSpace);

				    }
			    }
		    }
	    } 
        else 
        {
		    if (collisionShape.IsConcave())
		    {
			    if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
			    {
				    //BT_PROFILE("convexSweepbtBvhTriangleMesh");
				    BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
				    Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
				    Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
				    Vector3  convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
				    // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation

				    Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) *  MathUtil.BasisMatrix(ref convexToTrans);

				    BridgeTriangleConvexcastCallback tccb = new BridgeTriangleConvexcastCallback(castShape, ref convexFromTrans,ref convexToTrans,resultCallback,collisionObject,triangleMesh, ref colObjWorldTransform);
				    tccb.m_hitFraction = resultCallback.m_closestHitFraction;
				    Vector3 boxMinLocal = new Vector3();
                    Vector3 boxMaxLocal = new Vector3();
				    castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);
				    triangleMesh.PerformConvexCast(tccb,ref convexFromLocal,ref convexToLocal,ref boxMinLocal, ref boxMaxLocal);
			    } 
                else
			    {
				    //BT_PROFILE("convexSweepConcave");
				    ConcaveShape concaveShape = (ConcaveShape)collisionShape;
				    Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
				    Vector3 convexFromLocal = Vector3.Transform(convexFromTrans.Translation,worldTocollisionObject);
				    Vector3 convexToLocal = Vector3.Transform(convexToTrans.Translation,worldTocollisionObject);
				    // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                    Matrix rotationXform = MathUtil.BasisMatrix(ref worldTocollisionObject) * MathUtil.BasisMatrix(ref convexToTrans);

                    BridgeTriangleConvexcastCallback2 tccb = new BridgeTriangleConvexcastCallback2(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
				    tccb.m_hitFraction = resultCallback.m_closestHitFraction;
				    Vector3 boxMinLocal = new Vector3();
                    Vector3 boxMaxLocal = new Vector3();
				    castShape.GetAabb(ref rotationXform, ref boxMinLocal, ref boxMaxLocal);

				    Vector3  rayAabbMinLocal = convexFromLocal;
                    MathUtil.VectorMin(ref convexToLocal,ref rayAabbMinLocal);
                    //rayAabbMinLocal.setMin(convexToLocal);
				    Vector3  rayAabbMaxLocal = convexFromLocal;
                    //rayAabbMaxLocal.setMax(convexToLocal);
                    MathUtil.VectorMax(ref convexToLocal,ref rayAabbMaxLocal);

				    rayAabbMinLocal += boxMinLocal;
				    rayAabbMaxLocal += boxMaxLocal;
				    concaveShape.ProcessAllTriangles(tccb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
			    }
            } 
            else 
            {
			    ///@todo : use AABB tree or other BVH acceleration structure!
			    if (collisionShape.IsCompound())
			    {
                    //BT_PROFILE("convexSweepCompound");
                    CompoundShape compoundShape = (CompoundShape)collisionShape;
				    for (int i=0;i<compoundShape.GetNumChildShapes();i++)
				    {
					    Matrix childTrans = compoundShape.GetChildTransform(i);
					    CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
					    Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans);
					    // replace collision shape so that callback can determine the triangle
					    CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
					    collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);

                        LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
					    my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;


					    ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
						    collisionObject,
						    childCollisionShape,
						    ref childWorldTrans,
						    my_cb, allowedPenetration);
					    // restore
					    collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
				    }
			    }
		    }
	    }
    }
예제 #2
0
    public virtual void	RayTestSingle(ref Matrix rayFromTrans,ref Matrix rayToTrans,
					  CollisionObject collisionObject,
					  CollisionShape collisionShape,
					  ref Matrix colObjWorldTransform,
					  RayResultCallback resultCallback)
    {
	    SphereShape pointShape = new SphereShape(0.0f);
	    pointShape.Margin = 0f;
	    ConvexShape castShape = pointShape;

	    if (collisionShape.IsConvex())
	    {
    //		BT_PROFILE("rayTestConvex");
		    CastResult castResult = new CastResult();
		    castResult.m_fraction = resultCallback.m_closestHitFraction;

		    ConvexShape convexShape = (ConvexShape)collisionShape;
            VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
    //#define USE_SUBSIMPLEX_CONVEX_CAST 1
    //#ifdef USE_SUBSIMPLEX_CONVEX_CAST
            
            // FIXME - MAN - convexcat here seems to make big difference to forklift.
            SubSimplexConvexCast convexCaster = new SubSimplexConvexCast(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 = Vector3.TransformNormal(castResult.m_normal,rayFromTrans);
    //#endif //USE_SUBSIMPLEX_CONVEX_CAST

					    castResult.m_normal.Normalize();
					    LocalRayResult localRayResult = new LocalRayResult(
							    collisionObject,
							    null,
							    ref castResult.m_normal,
							    castResult.m_fraction
						    );

					    bool normalInWorldSpace = true;
					    resultCallback.AddSingleResult(localRayResult, normalInWorldSpace);

				    }
			    }
		    }
            castResult.Cleanup();
	    } 
        else 
        {
		    if (collisionShape.IsConcave())
		    {
    //			BT_PROFILE("rayTestConcave");
		    	if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape)
	    		{
				    ///optimized version for btBvhTriangleMeshShape
					BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
				    Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
				    Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
				    Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);

                    Matrix transform = Matrix.Identity;
				    BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(ref rayFromLocal,ref rayToLocal, resultCallback,collisionObject,triangleMesh,ref transform);
				    rcb.m_hitFraction = resultCallback.m_closestHitFraction;
				    triangleMesh.PerformRaycast(rcb,ref rayFromLocal,ref rayToLocal);
                    rcb.Cleanup();
			    } 
                else
			    {
				    //generic (slower) case
				    ConcaveShape concaveShape = (ConcaveShape)collisionShape;

				    Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);

				    Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
				    Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);

				    //ConvexCast::CastResult
                    Matrix transform = Matrix.Identity;
                    BridgeTriangleConcaveRaycastCallback rcb = new BridgeTriangleConcaveRaycastCallback(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape,ref transform);
				    rcb.m_hitFraction = resultCallback.m_closestHitFraction;

				    Vector3 rayAabbMinLocal = rayFromLocal;
				    MathUtil.VectorMin(ref rayToLocal,ref rayAabbMinLocal);
				    Vector3 rayAabbMaxLocal = rayFromLocal;
                    MathUtil.VectorMax(ref rayToLocal,ref rayAabbMaxLocal);

				    concaveShape.ProcessAllTriangles(rcb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
                    rcb.Cleanup();
			    }
		    } 
            else 
            {
                // BT_PROFILE("rayTestCompound");
			    ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
			    if (collisionShape.IsCompound())
			    {
				    CompoundShape compoundShape = (CompoundShape)(collisionShape);
				    int i=0;
				    for (i=0;i<compoundShape.GetNumChildShapes();i++)
				    {
					    Matrix childTrans = compoundShape.GetChildTransform(i);
					    CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
					    Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans) ;
					    // replace collision shape so that callback can determine the triangle
					    CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
					    collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);

                        LocalInfoAdder2 my_cb = new LocalInfoAdder2(i, resultCallback);
					    my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
					
					    RayTestSingle(ref rayFromTrans,ref rayToTrans,
						    collisionObject,
						    childCollisionShape,
						    ref childWorldTrans,
						    my_cb);
					    // restore
					    collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
                        my_cb.cleanup();
				    }
			    }
		    }
	    }
    }