Examples with ClosestPointInput com.bulletphysics.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput used on opensource projects

Example 1 with ClosestPointInput

use of com.bulletphysics.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput in project bdx by GoranM.

the class ConvexConvexAlgorithm method processCollision.

/**
	 * Convex-Convex collision algorithm.
	 */
@Override
public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        // swapped?
        manifoldPtr = dispatcher.getNewManifold(body0, body1);
        ownManifold = true;
    }
    resultOut.setPersistentManifold(manifoldPtr);
    //	#ifdef USE_BT_GJKEPA
    //		btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
    //		btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
    //		const btScalar				radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
    //		btGjkEpaSolver::sResults	results;
    //		if(btGjkEpaSolver::Collide(	shape0,body0->getWorldTransform(),
    //									shape1,body1->getWorldTransform(),
    //									radialmargin,results))
    //			{
    //			dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
    //			resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
    //			}
    //	#else
    ConvexShape min0 = (ConvexShape) body0.getCollisionShape();
    ConvexShape min1 = (ConvexShape) body1.getCollisionShape();
    ClosestPointInput input = pointInputsPool.get();
    input.init();
    // JAVA NOTE: original: TODO: if (dispatchInfo.m_useContinuous)
    gjkPairDetector.setMinkowskiA(min0);
    gjkPairDetector.setMinkowskiB(min1);
    input.maximumDistanceSquared = min0.getMargin() + min1.getMargin() + manifoldPtr.getContactBreakingThreshold();
    input.maximumDistanceSquared *= input.maximumDistanceSquared;
    //input.m_stackAlloc = dispatchInfo.m_stackAllocator;
    //	input.m_maximumDistanceSquared = btScalar(1e30);
    body0.getWorldTransform(input.transformA);
    body1.getWorldTransform(input.transformB);
    gjkPairDetector.getClosestPoints(input, resultOut, dispatchInfo.debugDraw);
    pointInputsPool.release(input);
    if (ownManifold) {
        resultOut.refreshContactPoints();
    }
}

Example 2 with ClosestPointInput

use of com.bulletphysics.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput in project bdx by GoranM.

the class GjkConvexCast method calcTimeOfImpact.

public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
    simplexSolver.reset();
    Stack stack = Stack.enter();
    int sp = stack.getSp();
    // compute linear velocity for this interval, to interpolate
    // assume no rotation/angular velocity, assert here?
    Vector3f linVelA = stack.allocVector3f();
    Vector3f linVelB = stack.allocVector3f();
    linVelA.sub(toA.origin, fromA.origin);
    linVelB.sub(toB.origin, fromB.origin);
    float radius = 0.001f;
    float lambda = 0f;
    Vector3f v = stack.allocVector3f();
    v.set(1f, 0f, 0f);
    int maxIter = MAX_ITERATIONS;
    Vector3f n = stack.allocVector3f();
    n.set(0f, 0f, 0f);
    boolean hasResult = false;
    Vector3f c = stack.allocVector3f();
    Vector3f r = stack.allocVector3f();
    r.sub(linVelA, linVelB);
    float lastLambda = lambda;
    //btScalar epsilon = btScalar(0.001);
    int numIter = 0;
    // first solution, using GJK
    Transform identityTrans = stack.allocTransform();
    identityTrans.setIdentity();
    //result.drawCoordSystem(sphereTr);
    PointCollector pointCollector = new PointCollector();
    // penetrationDepthSolver);		
    gjk.init(convexA, convexB, simplexSolver, null);
    ClosestPointInput input = pointInputsPool.get();
    input.init();
    try {
        // we don't use margins during CCD
        //	gjk.setIgnoreMargin(true);
        input.transformA.set(fromA);
        input.transformB.set(fromB);
        gjk.getClosestPoints(input, pointCollector, null);
        hasResult = pointCollector.hasResult;
        c.set(pointCollector.pointInWorld);
        if (hasResult) {
            float dist;
            dist = pointCollector.distance;
            n.set(pointCollector.normalOnBInWorld);
            // not close enough
            while (dist > radius) {
                numIter++;
                if (numIter > maxIter) {
                    // todo: report a failure
                    return false;
                }
                float dLambda = 0f;
                float projectedLinearVelocity = r.dot(n);
                dLambda = dist / (projectedLinearVelocity);
                lambda = lambda - dLambda;
                if (lambda > 1f) {
                    return false;
                }
                if (lambda < 0f) {
                    // todo: next check with relative epsilon
                    return false;
                }
                if (lambda <= lastLambda) {
                    return false;
                //n.setValue(0,0,0);
                //break;
                }
                lastLambda = lambda;
                // interpolate to next lambda
                result.debugDraw(lambda);
                VectorUtil.setInterpolate3(input.transformA.origin, fromA.origin, toA.origin, lambda);
                VectorUtil.setInterpolate3(input.transformB.origin, fromB.origin, toB.origin, lambda);
                gjk.getClosestPoints(input, pointCollector, null);
                if (pointCollector.hasResult) {
                    if (pointCollector.distance < 0f) {
                        result.fraction = lastLambda;
                        n.set(pointCollector.normalOnBInWorld);
                        result.normal.set(n);
                        result.hitPoint.set(pointCollector.pointInWorld);
                        stack.leave();
                        return true;
                    }
                    c.set(pointCollector.pointInWorld);
                    n.set(pointCollector.normalOnBInWorld);
                    dist = pointCollector.distance;
                } else {
                    // ??
                    return false;
                }
            }
            // don't report time of impact for motion away from the contact normal (or causes minor penetration)
            if (n.dot(r) >= -result.allowedPenetration) {
                return false;
            }
            result.fraction = lambda;
            result.normal.set(n);
            result.hitPoint.set(c);
            return true;
        }
        return false;
    } finally {
        stack.leave(sp);
        pointInputsPool.release(input);
    }
}