Examples with Stack com.bulletphysics.util.Stack used on opensource projects

Example 16 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class ConvexConcaveCollisionAlgorithm method processCollision.

@Override
public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    CollisionObject convexBody = isSwapped ? body1 : body0;
    CollisionObject triBody = isSwapped ? body0 : body1;
    if (triBody.getCollisionShape().isConcave()) {
        CollisionObject triOb = triBody;
        ConcaveShape concaveShape = (ConcaveShape) triOb.getCollisionShape();
        if (convexBody.getCollisionShape().isConvex()) {
            float collisionMarginTriangle = concaveShape.getMargin();
            resultOut.setPersistentManifold(btConvexTriangleCallback.manifoldPtr);
            btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
            // Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
            //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
            btConvexTriangleCallback.manifoldPtr.setBodies(convexBody, triBody);
            Stack stack = Stack.enter();
            concaveShape.processAllTriangles(btConvexTriangleCallback, btConvexTriangleCallback.getAabbMin(stack.allocVector3f()), btConvexTriangleCallback.getAabbMax(stack.allocVector3f()));
            resultOut.refreshContactPoints();
            stack.leave();
        }
    }
}

Example 17 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class ConvexConvexAlgorithm method calculateTimeOfImpact.

@Override
public float calculateTimeOfImpact(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Transform tmpTrans1 = stack.allocTransform();
    Transform tmpTrans2 = stack.allocTransform();
    // Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
    // Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
    // col0->m_worldTransform,
    float resultFraction = 1f;
    tmp.sub(col0.getInterpolationWorldTransform(tmpTrans1).origin, col0.getWorldTransform(tmpTrans2).origin);
    float squareMot0 = tmp.lengthSquared();
    tmp.sub(col1.getInterpolationWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
    float squareMot1 = tmp.lengthSquared();
    if (squareMot0 < col0.getCcdSquareMotionThreshold() && squareMot1 < col1.getCcdSquareMotionThreshold()) {
        return resultFraction;
    }
    if (disableCcd) {
        stack.leave();
        return 1f;
    }
    Transform tmpTrans3 = stack.allocTransform();
    Transform tmpTrans4 = stack.allocTransform();
    // 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 = (ConvexShape) col0.getCollisionShape();
        // todo: allow non-zero sphere sizes, for better approximation
        SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius());
        ConvexCast.CastResult result = new ConvexCast.CastResult();
        VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
        //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
        ///Simplification, one object is simplified as a sphere
        GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex);
        //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
        if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
            if (col0.getHitFraction() > result.fraction) {
                col0.setHitFraction(result.fraction);
            }
            if (col1.getHitFraction() > result.fraction) {
                col1.setHitFraction(result.fraction);
            }
            if (resultFraction > result.fraction) {
                resultFraction = result.fraction;
            }
        }
    }
    // Sphere (for convex0) against Convex1
    {
        ConvexShape convex1 = (ConvexShape) col1.getCollisionShape();
        // todo: allow non-zero sphere sizes, for better approximation
        SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius());
        ConvexCast.CastResult result = new ConvexCast.CastResult();
        VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
        //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
        ///Simplification, one object is simplified as a sphere
        GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex);
        //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
        if (ccd1.calcTimeOfImpact(col0.getWorldTransform(tmpTrans1), col0.getInterpolationWorldTransform(tmpTrans2), col1.getWorldTransform(tmpTrans3), col1.getInterpolationWorldTransform(tmpTrans4), result)) {
            if (col0.getHitFraction() > result.fraction) {
                col0.setHitFraction(result.fraction);
            }
            if (col1.getHitFraction() > result.fraction) {
                col1.setHitFraction(result.fraction);
            }
            if (resultFraction > result.fraction) {
                resultFraction = result.fraction;
            }
        }
    }
    stack.leave();
    return resultFraction;
}

Example 18 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class ConvexPlaneCollisionAlgorithm method processCollision.

@Override
public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        return;
    }
    Stack stack = Stack.enter();
    Transform tmpTrans = stack.allocTransform();
    CollisionObject convexObj = isSwapped ? body1 : body0;
    CollisionObject planeObj = isSwapped ? body0 : body1;
    ConvexShape convexShape = (ConvexShape) convexObj.getCollisionShape();
    StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.getCollisionShape();
    boolean hasCollision = false;
    Vector3f planeNormal = planeShape.getPlaneNormal(stack.allocVector3f());
    float planeConstant = planeShape.getPlaneConstant();
    Transform planeInConvex = stack.allocTransform();
    convexObj.getWorldTransform(planeInConvex);
    planeInConvex.inverse();
    planeInConvex.mul(planeObj.getWorldTransform(tmpTrans));
    Transform convexInPlaneTrans = stack.allocTransform();
    convexInPlaneTrans.inverse(planeObj.getWorldTransform(tmpTrans));
    convexInPlaneTrans.mul(convexObj.getWorldTransform(tmpTrans));
    Vector3f tmp = stack.allocVector3f();
    tmp.negate(planeNormal);
    planeInConvex.basis.transform(tmp);
    Vector3f vtx = convexShape.localGetSupportingVertex(tmp, stack.allocVector3f());
    Vector3f vtxInPlane = stack.alloc(vtx);
    convexInPlaneTrans.transform(vtxInPlane);
    float distance = (planeNormal.dot(vtxInPlane) - planeConstant);
    Vector3f vtxInPlaneProjected = stack.allocVector3f();
    tmp.scale(distance, planeNormal);
    vtxInPlaneProjected.sub(vtxInPlane, tmp);
    Vector3f vtxInPlaneWorld = stack.alloc(vtxInPlaneProjected);
    planeObj.getWorldTransform(tmpTrans).transform(vtxInPlaneWorld);
    hasCollision = distance < manifoldPtr.getContactBreakingThreshold();
    resultOut.setPersistentManifold(manifoldPtr);
    if (hasCollision) {
        // report a contact. internally this will be kept persistent, and contact reduction is done
        Vector3f normalOnSurfaceB = stack.alloc(planeNormal);
        planeObj.getWorldTransform(tmpTrans).basis.transform(normalOnSurfaceB);
        Vector3f pOnB = stack.alloc(vtxInPlaneWorld);
        resultOut.addContactPoint(normalOnSurfaceB, pOnB, distance);
    }
    if (ownManifold) {
        if (manifoldPtr.getNumContacts() != 0) {
            resultOut.refreshContactPoints();
        }
    }
    stack.leave();
}

Example 19 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class ConvexTriangleCallback method setTimeStepAndCounters.

public void setTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    this.dispatchInfoPtr = dispatchInfo;
    this.collisionMarginTriangle = collisionMarginTriangle;
    this.resultOut = resultOut;
    Stack stack = Stack.enter();
    // recalc aabbs
    Transform convexInTriangleSpace = stack.allocTransform();
    triBody.getWorldTransform(convexInTriangleSpace);
    convexInTriangleSpace.inverse();
    convexInTriangleSpace.mul(convexBody.getWorldTransform(stack.allocTransform()));
    CollisionShape convexShape = (CollisionShape) convexBody.getCollisionShape();
    //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
    convexShape.getAabb(convexInTriangleSpace, aabbMin, aabbMax);
    float extraMargin = collisionMarginTriangle;
    Vector3f extra = stack.allocVector3f();
    extra.set(extraMargin, extraMargin, extraMargin);
    aabbMax.add(extra);
    aabbMin.sub(extra);
    stack.leave();
}

Example 20 with Stack

use of com.bulletphysics.util.Stack in project bdx by GoranM.

the class ManifoldResult method addContactPoint.

public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth) {
    assert (manifoldPtr != null);
    if (depth > manifoldPtr.getContactBreakingThreshold()) {
        return;
    }
    boolean isSwapped = manifoldPtr.getBody0() != body0;
    Stack stack = Stack.enter();
    Vector3f pointA = stack.allocVector3f();
    pointA.scaleAdd(depth, normalOnBInWorld, pointInWorld);
    Vector3f localA = stack.allocVector3f();
    Vector3f localB = stack.allocVector3f();
    if (isSwapped) {
        rootTransB.invXform(pointA, localA);
        rootTransA.invXform(pointInWorld, localB);
    } else {
        rootTransA.invXform(pointA, localA);
        rootTransB.invXform(pointInWorld, localB);
    }
    ManifoldPoint newPt = pointsPool.get();
    newPt.init(localA, localB, normalOnBInWorld, depth);
    newPt.positionWorldOnA.set(pointA);
    newPt.positionWorldOnB.set(pointInWorld);
    int insertIndex = manifoldPtr.getCacheEntry(newPt);
    newPt.combinedFriction = calculateCombinedFriction(body0, body1);
    newPt.combinedRestitution = calculateCombinedRestitution(body0, body1);
    // BP mod, store contact triangles.
    newPt.partId0 = partId0;
    newPt.partId1 = partId1;
    newPt.index0 = index0;
    newPt.index1 = index1;
    /// todo, check this for any side effects
    if (insertIndex >= 0) {
        //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
        manifoldPtr.replaceContactPoint(newPt, insertIndex);
    } else {
        insertIndex = manifoldPtr.addManifoldPoint(newPt);
    }
    // User can override friction and/or restitution
    if (BulletGlobals.getContactAddedCallback() != null && // and if either of the two bodies requires custom material
    ((body0.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0 || (body1.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0)) {
        //experimental feature info, for per-triangle material etc.
        CollisionObject obj0 = isSwapped ? body1 : body0;
        CollisionObject obj1 = isSwapped ? body0 : body1;
        BulletGlobals.getContactAddedCallback().contactAdded(manifoldPtr.getContactPoint(insertIndex), obj0, partId0, index0, obj1, partId1, index1);
    }
    pointsPool.release(newPt);
    stack.leave();
}