public void ProcessTriangle(ObjectArray<Vector3> triangle, int partId, int triangleIndex)
		{
			//do a swept sphere for now
			Matrix ident = Matrix.Identity;
			CastResult castResult = new CastResult();
			castResult.m_fraction = m_hitFraction;
			SphereShape	pointShape = new SphereShape(m_ccdSphereRadius);
			TriangleShape triShape = new TriangleShape(triangle[0],triangle[1],triangle[2]);
			VoronoiSimplexSolver	simplexSolver = new VoronoiSimplexSolver();
			SubSimplexConvexCast convexCaster = new SubSimplexConvexCast(pointShape,triShape,simplexSolver);
			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
			//local space?

			if (convexCaster.CalcTimeOfImpact(ref m_ccdSphereFromTrans,ref m_ccdSphereToTrans,
			                                  ref ident,ref ident,castResult))
			{
				if (m_hitFraction > castResult.m_fraction)
				{
					m_hitFraction = castResult.m_fraction;
				}
			}

		}
Пример #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();
				    }
			    }
		    }
	    }
    }