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

Example 21 with Stack

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

the class SimulationIslandManager method buildAndProcessIslands.

public void buildAndProcessIslands(Dispatcher dispatcher, ObjectArrayList<CollisionObject> collisionObjects, IslandCallback callback) {
    buildIslands(dispatcher, collisionObjects);
    int endIslandIndex = 1;
    int startIslandIndex;
    int numElem = getUnionFind().getNumElements();
    BulletStats.pushProfile("processIslands");
    Stack stack = Stack.enter();
    int sp = stack.getSp();
    try {
        //#ifndef SPLIT_ISLANDS
        //btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
        //
        //callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
        //#else
        // Sort manifolds, based on islands
        // Sort the vector using predicate and std::sort
        //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
        int numManifolds = islandmanifold.size();
        // we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
        //islandmanifold.heapSort(btPersistentManifoldSortPredicate());
        // JAVA NOTE: memory optimized sorting with caching of temporary array
        //Collections.sort(islandmanifold, persistentManifoldComparator);
        MiscUtil.quickSort(islandmanifold, persistentManifoldComparator);
        // now process all active islands (sets of manifolds for now)
        int startManifoldIndex = 0;
        int endManifoldIndex = 1;
        // traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
        for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) {
            int islandId = getUnionFind().getElement(startIslandIndex).id;
            boolean islandSleeping = false;
            for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) {
                int i = getUnionFind().getElement(endIslandIndex).sz;
                CollisionObject colObj0 = collisionObjects.getQuick(i);
                islandBodies.add(colObj0);
                if (!colObj0.isActive()) {
                    islandSleeping = true;
                }
            }
            // find the accompanying contact manifold for this islandId
            int numIslandManifolds = 0;
            //ObjectArrayList<PersistentManifold> startManifold = null;
            int startManifold_idx = -1;
            if (startManifoldIndex < numManifolds) {
                int curIslandId = getIslandId(islandmanifold.getQuick(startManifoldIndex));
                if (curIslandId == islandId) {
                    //startManifold = &m_islandmanifold[startManifoldIndex];
                    //startManifold = islandmanifold.subList(startManifoldIndex, islandmanifold.size());
                    startManifold_idx = startManifoldIndex;
                    for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(islandmanifold.getQuick(endManifoldIndex))); endManifoldIndex++) {
                    }
                    // Process the actual simulation, only if not sleeping/deactivated
                    numIslandManifolds = endManifoldIndex - startManifoldIndex;
                }
            }
            if (!islandSleeping) {
                callback.processIsland(islandBodies, islandBodies.size(), islandmanifold, startManifold_idx, numIslandManifolds, islandId);
            //printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
            }
            if (numIslandManifolds != 0) {
                startManifoldIndex = endManifoldIndex;
            }
            islandBodies.clear();
        }
    //#endif //SPLIT_ISLANDS
    } finally {
        BulletStats.popProfile();
        stack.leave(sp);
    }
}

Example 22 with Stack

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

the class SphereSphereCollisionAlgorithm method processCollision.

@Override
public void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    if (manifoldPtr == null) {
        return;
    }
    Stack stack = Stack.enter();
    Transform tmpTrans1 = stack.allocTransform();
    Transform tmpTrans2 = stack.allocTransform();
    resultOut.setPersistentManifold(manifoldPtr);
    SphereShape sphere0 = (SphereShape) col0.getCollisionShape();
    SphereShape sphere1 = (SphereShape) col1.getCollisionShape();
    Vector3f diff = stack.allocVector3f();
    diff.sub(col0.getWorldTransform(tmpTrans1).origin, col1.getWorldTransform(tmpTrans2).origin);
    float len = diff.length();
    float radius0 = sphere0.getRadius();
    float radius1 = sphere1.getRadius();
    // if distance positive, don't generate a new contact
    if (len > (radius0 + radius1)) {
        //#ifndef CLEAR_MANIFOLD
        resultOut.refreshContactPoints();
        //#endif //CLEAR_MANIFOLD
        return;
    }
    // distance (negative means penetration)
    float dist = len - (radius0 + radius1);
    Vector3f normalOnSurfaceB = stack.allocVector3f();
    normalOnSurfaceB.set(1f, 0f, 0f);
    if (len > BulletGlobals.FLT_EPSILON) {
        normalOnSurfaceB.scale(1f / len, diff);
    }
    Vector3f tmp = stack.allocVector3f();
    // point on A (worldspace)
    Vector3f pos0 = stack.allocVector3f();
    tmp.scale(radius0, normalOnSurfaceB);
    pos0.sub(col0.getWorldTransform(tmpTrans1).origin, tmp);
    // point on B (worldspace)
    Vector3f pos1 = stack.allocVector3f();
    tmp.scale(radius1, normalOnSurfaceB);
    pos1.add(col1.getWorldTransform(tmpTrans2).origin, tmp);
    // report a contact. internally this will be kept persistent, and contact reduction is done
    resultOut.addContactPoint(normalOnSurfaceB, pos1, dist);
    //#ifndef CLEAR_MANIFOLD
    resultOut.refreshContactPoints();
    //#endif //CLEAR_MANIFOLD
    stack.leave();
}

Example 23 with Stack

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

the class GjkEpaSolver method collide.

public boolean collide(ConvexShape shape0, Transform wtrs0, ConvexShape shape1, Transform wtrs1, float radialmargin, /*,
			btStackAlloc* stackAlloc*/
Results results) {
    Stack stack = Stack.enter();
    int sp = stack.getSp();
    // Initialize
    results.witnesses[0].set(0f, 0f, 0f);
    results.witnesses[1].set(0f, 0f, 0f);
    results.normal.set(0f, 0f, 0f);
    results.depth = 0;
    results.status = ResultsStatus.Separated;
    results.epa_iterations = 0;
    results.gjk_iterations = 0;
    /* Use GJK to locate origin		*/
    gjk.init(/*stackAlloc,*/
    wtrs0.basis, wtrs0.origin, shape0, wtrs1.basis, wtrs1.origin, shape1, radialmargin + EPA_accuracy);
    try {
        boolean collide = gjk.SearchOrigin();
        results.gjk_iterations = gjk.iterations + 1;
        if (collide) {
            /* Then EPA for penetration depth	*/
            EPA epa = new EPA(gjk);
            float pd = epa.EvaluatePD();
            results.epa_iterations = epa.iterations + 1;
            if (pd > 0) {
                results.status = ResultsStatus.Penetrating;
                results.normal.set(epa.normal);
                results.depth = pd;
                results.witnesses[0].set(epa.nearest[0]);
                results.witnesses[1].set(epa.nearest[1]);
                return (true);
            } else {
                if (epa.failed) {
                    results.status = ResultsStatus.EPA_Failed;
                }
            }
        } else {
            if (gjk.failed) {
                results.status = ResultsStatus.GJK_Failed;
            }
        }
        return (false);
    } finally {
        stack.leave(sp);
        gjk.destroy();
    }
}

Example 24 with Stack

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

the class PersistentManifold method refreshContactPoints.

/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
public void refreshContactPoints(Transform trA, Transform trB) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    int i;
    // first refresh worldspace positions and distance
    for (i = getNumContacts() - 1; i >= 0; i--) {
        ManifoldPoint manifoldPoint = pointCache[i];
        manifoldPoint.positionWorldOnA.set(manifoldPoint.localPointA);
        trA.transform(manifoldPoint.positionWorldOnA);
        manifoldPoint.positionWorldOnB.set(manifoldPoint.localPointB);
        trB.transform(manifoldPoint.positionWorldOnB);
        tmp.set(manifoldPoint.positionWorldOnA);
        tmp.sub(manifoldPoint.positionWorldOnB);
        manifoldPoint.distance1 = tmp.dot(manifoldPoint.normalWorldOnB);
        manifoldPoint.lifeTime++;
    }
    // then 
    float distance2d;
    Vector3f projectedDifference = stack.allocVector3f(), projectedPoint = stack.allocVector3f();
    for (i = getNumContacts() - 1; i >= 0; i--) {
        ManifoldPoint manifoldPoint = pointCache[i];
        // contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
        if (!validContactDistance(manifoldPoint)) {
            removeContactPoint(i);
        } else {
            // contact also becomes invalid when relative movement orthogonal to normal exceeds margin
            tmp.scale(manifoldPoint.distance1, manifoldPoint.normalWorldOnB);
            projectedPoint.sub(manifoldPoint.positionWorldOnA, tmp);
            projectedDifference.sub(manifoldPoint.positionWorldOnB, projectedPoint);
            distance2d = projectedDifference.dot(projectedDifference);
            if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) {
                removeContactPoint(i);
            } else {
                // contact point processed callback
                if (BulletGlobals.getContactProcessedCallback() != null) {
                    BulletGlobals.getContactProcessedCallback().contactProcessed(manifoldPoint, body0, body1);
                }
            }
        }
    }
    //#ifdef DEBUG_PERSISTENCY
    //	DebugPersistency();
    //#endif //
    stack.leave();
}

Example 25 with Stack

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

the class PersistentManifold method getCacheEntry.

public int getCacheEntry(ManifoldPoint newPoint) {
    float shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
    int size = getNumContacts();
    int nearestPoint = -1;
    Stack stack = Stack.enter();
    Vector3f diffA = stack.allocVector3f();
    for (int i = 0; i < size; i++) {
        ManifoldPoint mp = pointCache[i];
        diffA.sub(mp.localPointA, newPoint.localPointA);
        float distToManiPoint = diffA.dot(diffA);
        if (distToManiPoint < shortestDist) {
            shortestDist = distToManiPoint;
            nearestPoint = i;
        }
    }
    stack.leave();
    return nearestPoint;
}