Exemplo n.º 1
0
 public virtual bool CalcTimeOfImpact(IndexedMatrix fromA, IndexedMatrix toA, IndexedMatrix fromB, IndexedMatrix toB, CastResult result)
 {
     return CalcTimeOfImpact(ref fromA, ref toA, ref fromB, ref toB, result);
 }
Exemplo n.º 2
0
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            m_simplexSolver.Reset();

            /// compute linear velocity for this interval, to interpolate
            //assume no rotation/angular velocity, assert here?
            IndexedVector3 linVelA, linVelB;
            linVelA = toA._origin - fromA._origin;
            linVelB = toB._origin - fromB._origin;

            float radius = 0.001f;
            float lambda = 0f;
            IndexedVector3 v = new IndexedVector3(1, 0, 0);

            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n = IndexedVector3.Zero;
            bool hasResult = false;
            IndexedVector3 c;
            IndexedVector3 r = (linVelA - linVelB);

            float lastLambda = lambda;
            //float epsilon = float(0.001);

            int numIter = 0;
            //first solution, using GJK


            IndexedMatrix identityTrans = IndexedMatrix.Identity;

        //	result.drawCoordSystem(sphereTr);

            PointCollector pointCollector = new PointCollector();


            using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
            {
                gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);		
                ClosestPointInput input = ClosestPointInput.Default();

                //we don't use margins during CCD
                //	gjk.setIgnoreMargin(true);

                input.m_transformA = fromA;
                input.m_transformB = fromB;
                gjk.GetClosestPoints(ref input, pointCollector, null, false);

                hasResult = pointCollector.m_hasResult;
                c = pointCollector.m_pointInWorld;

                if (hasResult)
                {
                    float dist = pointCollector.m_distance;
                    n = pointCollector.m_normalOnBInWorld;

                    //not close enough
                    while (dist > radius)
                    {
                        numIter++;
                        if (numIter > maxIter)
                        {
                            return false; //todo: report a failure
                        }
                        float dLambda = 0f;

                        float projectedLinearVelocity = IndexedVector3.Dot(r, n);

                        dLambda = dist / (projectedLinearVelocity);

                        lambda = lambda - dLambda;

                        if (lambda > 1f || lambda < 0f)
                        {
                            return false;
                        }

                        //todo: next check with relative epsilon
                        if (lambda <= lastLambda)
                        {
                            return false;
                            //n.setValue(0,0,0);
                            //break;
                        }
                        lastLambda = lambda;

                        //interpolate to next lambda
                        result.DebugDraw(lambda);
                        input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
                        input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);

                        gjk.GetClosestPoints(ref input, pointCollector, null, false);
                        if (pointCollector.m_hasResult)
                        {
                            if (pointCollector.m_distance < 0f)
                            {
                                result.m_fraction = lastLambda;
                                n = pointCollector.m_normalOnBInWorld;
                                result.m_normal = n;
                                result.m_hitPoint = pointCollector.m_pointInWorld;

                                return true;
                            }
                            c = pointCollector.m_pointInWorld;
                            n = pointCollector.m_normalOnBInWorld;
                            dist = pointCollector.m_distance;

                        }
                        else
                        {
                            //??
                            return false;
                        }
                    }

                    //is n normalized?
                    //don't report time of impact for motion away from the contact normal (or causes minor penetration)
                    if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
                    {
                        return false;
                    }

                    result.m_fraction = lambda;
                    result.m_normal = n;
                    result.m_hitPoint = c;
                    return true;
                }
            }
            return false;
        }
Exemplo n.º 3
0
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            m_simplexSolver.Reset();

            /// compute linear velocity for this interval, to interpolate
            //assume no rotation/angular velocity, assert here?
            IndexedVector3 linVelA, linVelB;

            linVelA = toA._origin - fromA._origin;
            linVelB = toB._origin - fromB._origin;

            float          radius = 0.001f;
            float          lambda = 0f;
            IndexedVector3 v      = new IndexedVector3(1, 0, 0);

            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n         = IndexedVector3.Zero;
            bool           hasResult = false;
            IndexedVector3 c;
            IndexedVector3 r = (linVelA - linVelB);

            float lastLambda = lambda;
            //float epsilon = float(0.001);

            int numIter = 0;
            //first solution, using GJK


            IndexedMatrix identityTrans = IndexedMatrix.Identity;

            //	result.drawCoordSystem(sphereTr);

            PointCollector pointCollector = new PointCollector();


            using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
            {
                gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
                ClosestPointInput input = ClosestPointInput.Default();

                //we don't use margins during CCD
                //	gjk.setIgnoreMargin(true);

                input.m_transformA = fromA;
                input.m_transformB = fromB;
                gjk.GetClosestPoints(ref input, pointCollector, null, false);

                hasResult = pointCollector.m_hasResult;
                c         = pointCollector.m_pointInWorld;

                if (hasResult)
                {
                    float dist = pointCollector.m_distance;
                    n = pointCollector.m_normalOnBInWorld;

                    //not close enough
                    while (dist > radius)
                    {
                        numIter++;
                        if (numIter > maxIter)
                        {
                            return(false); //todo: report a failure
                        }
                        float dLambda = 0f;

                        float projectedLinearVelocity = IndexedVector3.Dot(r, n);

                        dLambda = dist / (projectedLinearVelocity);

                        lambda = lambda - dLambda;

                        if (lambda > 1f || lambda < 0f)
                        {
                            return(false);
                        }

                        //todo: next check with relative epsilon
                        if (lambda <= lastLambda)
                        {
                            return(false);
                            //n.setValue(0,0,0);
                            //break;
                        }
                        lastLambda = lambda;

                        //interpolate to next lambda
                        result.DebugDraw(lambda);
                        input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
                        input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);

                        gjk.GetClosestPoints(ref input, pointCollector, null, false);
                        if (pointCollector.m_hasResult)
                        {
                            if (pointCollector.m_distance < 0f)
                            {
                                result.m_fraction = lastLambda;
                                n = pointCollector.m_normalOnBInWorld;
                                result.m_normal   = n;
                                result.m_hitPoint = pointCollector.m_pointInWorld;

                                return(true);
                            }
                            c    = pointCollector.m_pointInWorld;
                            n    = pointCollector.m_normalOnBInWorld;
                            dist = pointCollector.m_distance;
                        }
                        else
                        {
                            //??
                            return(false);
                        }
                    }

                    //is n normalized?
                    //don't report time of impact for motion away from the contact normal (or causes minor penetration)
                    if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
                    {
                        return(false);
                    }

                    result.m_fraction = lambda;
                    result.m_normal   = n;
                    result.m_hitPoint = c;
                    return(true);
                }
            }
            return(false);
        }
Exemplo n.º 4
0
        public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            ///body0.m_worldTransform,
            float resultFraction = 1.0f;


            float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
            float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();

            if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
                squareMot1 < body1.GetCcdSquareMotionThreshold())
            {
                return(resultFraction);
            }

            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)


            /// Convex0 against sphere for Convex1
            {
                ConvexShape convex0 = body0.GetCollisionShape() as ConvexShape;

                SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
                sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(convex0, sphere1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                                              body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {
                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }

                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }

                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere1);
                    result.Cleanup();
                }
            }

            /// Sphere (for convex0) against Convex1
            {
                ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;

                SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
                sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(sphere0, convex1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                                              body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {
                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }

                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }

                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere0);
                    result.Cleanup();
                }
            }

            return(resultFraction);
        }
Exemplo n.º 5
0
 public virtual bool CalcTimeOfImpact(IndexedMatrix fromA, IndexedMatrix toA, IndexedMatrix fromB, IndexedMatrix toB, CastResult result)
 {
     return(CalcTimeOfImpact(ref fromA, ref toA, ref fromB, ref toB, result));
 }
Exemplo n.º 6
0
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            /// compute linear and angular velocity for this interval, to interpolate
            IndexedVector3 linVelA, angVelA, linVelB, angVelB;
            TransformUtil.CalculateVelocity(ref fromA, ref toA, 1f, out linVelA, out angVelA);
            TransformUtil.CalculateVelocity(ref fromB, ref toB, 1f, out linVelB, out angVelB);

            float boundingRadiusA = m_convexA.GetAngularMotionDisc();
            float boundingRadiusB = m_convexB1 != null ? m_convexB1.GetAngularMotionDisc() : 0.0f;

            float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
            IndexedVector3 relLinVel = (linVelB - linVelA);

            float relLinVelocLength = relLinVel.Length();

            if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
            {
                return false;
            }


            float lambda = 0f;
            IndexedVector3 v = new IndexedVector3(1, 0, 0);

            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n = IndexedVector3.Zero;

            bool hasResult = false;
            IndexedVector3 c;

            float lastLambda = lambda;
            //float epsilon = float(0.001);

            int numIter = 0;
            //first solution, using GJK


            float radius = 0.001f;

            //	result.drawCoordSystem(sphereTr);

            PointCollector pointCollector1 = new PointCollector();

            {
                ComputeClosestPoints(ref fromA, ref fromB, pointCollector1);

                hasResult = pointCollector1.m_hasResult;
                c = pointCollector1.m_pointInWorld;
            }

            if (hasResult)
            {
                float dist = pointCollector1.m_distance + result.m_allowedPenetration;
 
                n = pointCollector1.m_normalOnBInWorld;

                float projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
                if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
                {
                    return false;
                }

                //not close enough
                while (dist > radius)
                {
                    if (result.m_debugDrawer != null)
                    {
                        IndexedVector3 colour = new IndexedVector3(1, 1, 1);
                        result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
                    }
                    float dLambda = 0f;

                    projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);

                    //don't report time of impact for motion away from the contact normal (or causes minor penetration)
                    if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
                    {
                        return false;
                    }
                    dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

                    lambda = lambda + dLambda;

                    if (lambda > 1f || lambda < 0f)
                    {
                        return false;
                    }


                    //todo: next check with relative epsilon
                    if (lambda <= lastLambda)
                    {
                        return false;
                        //n.setValue(0,0,0);
                    }

                    lastLambda = lambda;

                    //interpolate to next lambda
                    IndexedMatrix interpolatedTransA = IndexedMatrix.Identity, interpolatedTransB = IndexedMatrix.Identity, relativeTrans = IndexedMatrix.Identity;

                    TransformUtil.IntegrateTransform(ref fromA, ref linVelA, ref angVelA, lambda, out interpolatedTransA);
                    TransformUtil.IntegrateTransform(ref fromB, ref linVelB, ref angVelB, lambda, out interpolatedTransB);
                    //relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
                    relativeTrans = interpolatedTransB.InverseTimes(ref interpolatedTransA);
                    if (result.m_debugDrawer != null)
                    {
                        result.m_debugDrawer.DrawSphere(interpolatedTransA._origin, 0.2f, new IndexedVector3(1, 0, 0));
                    }
                    result.DebugDraw(lambda);

                    PointCollector pointCollector = new PointCollector();
                    ComputeClosestPoints(ref interpolatedTransA, ref interpolatedTransB, pointCollector);
                    if (pointCollector.m_hasResult)
                    {
                        dist = pointCollector.m_distance + result.m_allowedPenetration;

                        c = pointCollector.m_pointInWorld;
                        n = pointCollector.m_normalOnBInWorld;
                        dist = pointCollector.m_distance;
                    }
                    else
                    {
                        result.ReportFailure(-1, numIter);
                        return false;
                    }
                    numIter++;
                    if (numIter > maxIter)
                    {
                        result.ReportFailure(-2, numIter);
                        return false;
                    }


                }

                result.m_fraction = lambda;
                result.m_normal = n;
                result.m_hitPoint = c;
                return true;
            }

            return false;

        }
Exemplo n.º 7
0
        ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
        ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            m_simplexSolver.Reset();

            IndexedVector3 linVelA = toA._origin - fromA._origin;
            IndexedVector3 linVelB = toB._origin - fromB._origin;

            float lambda = 0f;

            IndexedMatrix interpolatedTransA = fromA;
            IndexedMatrix interpolatedTransB = fromB;

            ///take relative motion
            IndexedVector3 r = (linVelA - linVelB);
            IndexedVector3 v;

            IndexedVector3 supVertexA = fromA * m_convexA.LocalGetSupportingVertex(-r * fromA._basis);
            IndexedVector3 supVertexB = fromB * m_convexB.LocalGetSupportingVertex(r * fromB._basis);
            v = supVertexA - supVertexB;
            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n = IndexedVector3.Zero;

            bool hasResult = false;
            IndexedVector3 c;

            float lastLambda = lambda;


            float dist2 = v.LengthSquared();
            float epsilon = 0.0001f;
            IndexedVector3 w, p;
            float VdotR;

            while ((dist2 > epsilon) && (maxIter-- > 0))
            {
                supVertexA = interpolatedTransA * (m_convexA.LocalGetSupportingVertex(-v * interpolatedTransA._basis));
                supVertexB = interpolatedTransB * (m_convexB.LocalGetSupportingVertex(v * interpolatedTransB._basis));

                w = supVertexA - supVertexB;

                float VdotW = IndexedVector3.Dot(v, w);

                if (lambda > 1.0f)
                {
                    return false;
                }

                if (VdotW > 0f)
                {
                    VdotR = IndexedVector3.Dot(v, r);

                    if (VdotR >= -(MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                    {
                        return false;
                    }
                    else
                    {
                        lambda = lambda - VdotW / VdotR;
                        //interpolate to next lambda
                        //	x = s + lambda * r;

                        interpolatedTransA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
                        interpolatedTransB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
                        //m_simplexSolver.reset();
                        //check next line
                        w = supVertexA - supVertexB;
                        lastLambda = lambda;
                        n = v;
                        hasResult = true;
                    }
                }
                ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
                if (!m_simplexSolver.InSimplex(ref w))
                {
                    m_simplexSolver.AddVertex(ref w, ref supVertexA, ref supVertexB);
                }

                if (m_simplexSolver.Closest(out v))
                {
                    dist2 = v.LengthSquared();
                    hasResult = true;
                    //todo: check this normal for validity
                    //n=v;
                    //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
                    //printf("DIST2=%f\n",dist2);
                    //printf("numverts = %i\n",m_simplexSolver.numVertices());
                }
                else
                {
                    dist2 = 0f;
                }
            }

            //int numiter = MAX_ITERATIONS - maxIter;
            //	printf("number of iterations: %d", numiter);

            //don't report a time of impact when moving 'away' from the hitnormal

            result.m_fraction = lambda;
            if (n.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
            {
                result.m_normal = IndexedVector3.Normalize(n);
            }
            else
            {
                result.m_normal = IndexedVector3.Zero;
            }

            //don't report time of impact for motion away from the contact normal (or causes minor penetration)
            if (IndexedVector3.Dot(result.m_normal, r) >= -result.m_allowedPenetration)
            {
                return false;
            }

            IndexedVector3 hitA, hitB;
            m_simplexSolver.ComputePoints(out hitA, out hitB);
            result.m_hitPoint = hitB;
            return true;

        }
Exemplo n.º 8
0
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            /// compute linear and angular velocity for this interval, to interpolate
            IndexedVector3 linVelA, angVelA, linVelB, angVelB;

            TransformUtil.CalculateVelocity(ref fromA, ref toA, 1f, out linVelA, out angVelA);
            TransformUtil.CalculateVelocity(ref fromB, ref toB, 1f, out linVelB, out angVelB);

            float boundingRadiusA = m_convexA.GetAngularMotionDisc();
            float boundingRadiusB = m_convexB1 != null?m_convexB1.GetAngularMotionDisc() : 0.0f;

            float          maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
            IndexedVector3 relLinVel = (linVelB - linVelA);

            float relLinVelocLength = relLinVel.Length();

            if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
            {
                return(false);
            }


            float          lambda = 0f;
            IndexedVector3 v      = new IndexedVector3(1, 0, 0);

            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n = IndexedVector3.Zero;

            bool           hasResult = false;
            IndexedVector3 c;

            float lastLambda = lambda;
            //float epsilon = float(0.001);

            int numIter = 0;
            //first solution, using GJK


            float radius = 0.001f;

            //	result.drawCoordSystem(sphereTr);

            PointCollector pointCollector1 = new PointCollector();

            {
                ComputeClosestPoints(ref fromA, ref fromB, pointCollector1);

                hasResult = pointCollector1.m_hasResult;
                c         = pointCollector1.m_pointInWorld;
            }

            if (hasResult)
            {
                float dist = pointCollector1.m_distance + result.m_allowedPenetration;

                n = pointCollector1.m_normalOnBInWorld;

                float projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);
                if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
                {
                    return(false);
                }

                //not close enough
                while (dist > radius)
                {
                    if (result.m_debugDrawer != null)
                    {
                        IndexedVector3 colour = new IndexedVector3(1, 1, 1);
                        result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
                    }
                    float dLambda = 0f;

                    projectedLinearVelocity = IndexedVector3.Dot(relLinVel, n);

                    //don't report time of impact for motion away from the contact normal (or causes minor penetration)
                    if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= MathUtil.SIMD_EPSILON)
                    {
                        return(false);
                    }
                    dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

                    lambda = lambda + dLambda;

                    if (lambda > 1f || lambda < 0f)
                    {
                        return(false);
                    }


                    //todo: next check with relative epsilon
                    if (lambda <= lastLambda)
                    {
                        return(false);
                        //n.setValue(0,0,0);
                    }

                    lastLambda = lambda;

                    //interpolate to next lambda
                    IndexedMatrix interpolatedTransA = IndexedMatrix.Identity, interpolatedTransB = IndexedMatrix.Identity, relativeTrans = IndexedMatrix.Identity;

                    TransformUtil.IntegrateTransform(ref fromA, ref linVelA, ref angVelA, lambda, out interpolatedTransA);
                    TransformUtil.IntegrateTransform(ref fromB, ref linVelB, ref angVelB, lambda, out interpolatedTransB);
                    //relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
                    relativeTrans = interpolatedTransB.InverseTimes(ref interpolatedTransA);
                    if (result.m_debugDrawer != null)
                    {
                        result.m_debugDrawer.DrawSphere(interpolatedTransA._origin, 0.2f, new IndexedVector3(1, 0, 0));
                    }
                    result.DebugDraw(lambda);

                    PointCollector pointCollector = new PointCollector();
                    ComputeClosestPoints(ref interpolatedTransA, ref interpolatedTransB, pointCollector);
                    if (pointCollector.m_hasResult)
                    {
                        dist = pointCollector.m_distance + result.m_allowedPenetration;

                        c    = pointCollector.m_pointInWorld;
                        n    = pointCollector.m_normalOnBInWorld;
                        dist = pointCollector.m_distance;
                    }
                    else
                    {
                        result.ReportFailure(-1, numIter);
                        return(false);
                    }
                    numIter++;
                    if (numIter > maxIter)
                    {
                        result.ReportFailure(-2, numIter);
                        return(false);
                    }
                }

                result.m_fraction = lambda;
                result.m_normal   = n;
                result.m_hitPoint = c;
                return(true);
            }

            return(false);
        }
Exemplo n.º 9
0
        ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
        ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            m_simplexSolver.Reset();

            IndexedVector3 linVelA = toA._origin - fromA._origin;
            IndexedVector3 linVelB = toB._origin - fromB._origin;

            float lambda = 0f;

            IndexedMatrix interpolatedTransA = fromA;
            IndexedMatrix interpolatedTransB = fromB;

            ///take relative motion
            IndexedVector3 r = (linVelA - linVelB);
            IndexedVector3 v;

            IndexedVector3 supVertexA = fromA * m_convexA.LocalGetSupportingVertex(-r * fromA._basis);
            IndexedVector3 supVertexB = fromB * m_convexB.LocalGetSupportingVertex(r * fromB._basis);

            v = supVertexA - supVertexB;
            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n = IndexedVector3.Zero;

            bool           hasResult = false;
            IndexedVector3 c;

            float lastLambda = lambda;


            float          dist2 = v.LengthSquared();
            float          epsilon = 0.0001f;
            IndexedVector3 w, p;
            float          VdotR;

            while ((dist2 > epsilon) && (maxIter-- > 0))
            {
                supVertexA = interpolatedTransA * (m_convexA.LocalGetSupportingVertex(-v * interpolatedTransA._basis));
                supVertexB = interpolatedTransB * (m_convexB.LocalGetSupportingVertex(v * interpolatedTransB._basis));

                w = supVertexA - supVertexB;

                float VdotW = IndexedVector3.Dot(v, w);

                if (lambda > 1.0f)
                {
                    return(false);
                }

                if (VdotW > 0f)
                {
                    VdotR = IndexedVector3.Dot(v, r);

                    if (VdotR >= -(MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                    {
                        return(false);
                    }
                    else
                    {
                        lambda = lambda - VdotW / VdotR;
                        //interpolate to next lambda
                        //	x = s + lambda * r;

                        interpolatedTransA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
                        interpolatedTransB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);
                        //m_simplexSolver.reset();
                        //check next line
                        w          = supVertexA - supVertexB;
                        lastLambda = lambda;
                        n          = v;
                        hasResult  = true;
                    }
                }
                ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
                if (!m_simplexSolver.InSimplex(ref w))
                {
                    m_simplexSolver.AddVertex(ref w, ref supVertexA, ref supVertexB);
                }

                if (m_simplexSolver.Closest(out v))
                {
                    dist2     = v.LengthSquared();
                    hasResult = true;
                    //todo: check this normal for validity
                    //n=v;
                    //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
                    //printf("DIST2=%f\n",dist2);
                    //printf("numverts = %i\n",m_simplexSolver.numVertices());
                }
                else
                {
                    dist2 = 0f;
                }
            }

            //int numiter = MAX_ITERATIONS - maxIter;
            //	printf("number of iterations: %d", numiter);

            //don't report a time of impact when moving 'away' from the hitnormal

            result.m_fraction = lambda;
            if (n.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
            {
                result.m_normal = IndexedVector3.Normalize(n);
            }
            else
            {
                result.m_normal = IndexedVector3.Zero;
            }

            //don't report time of impact for motion away from the contact normal (or causes minor penetration)
            if (IndexedVector3.Dot(result.m_normal, r) >= -result.m_allowedPenetration)
            {
                return(false);
            }

            IndexedVector3 hitA, hitB;

            m_simplexSolver.ComputePoints(out hitA, out hitB);
            result.m_hitPoint = hitB;
            return(true);
        }