Examples with PersistentManifold com.bulletphysics.collision.narrowphase.PersistentManifold used on opensource projects

Example 6 with PersistentManifold

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

the class DiscreteDynamicsWorld method debugDrawWorld.

@Override
public void debugDrawWorld() {
    Stack stack = Stack.enter();
    if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) {
        int numManifolds = getDispatcher().getNumManifolds();
        Vector3f color = stack.allocVector3f();
        color.set(0f, 0f, 0f);
        for (int i = 0; i < numManifolds; i++) {
            PersistentManifold contactManifold = getDispatcher().getManifoldByIndexInternal(i);
            //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
            //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
            int numContacts = contactManifold.getNumContacts();
            for (int j = 0; j < numContacts; j++) {
                ManifoldPoint cp = contactManifold.getContactPoint(j);
                getDebugDrawer().drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color);
            }
        }
    }
    if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & (DebugDrawModes.DRAW_WIREFRAME | DebugDrawModes.DRAW_AABB)) != 0) {
        int i;
        Transform tmpTrans = stack.allocTransform();
        Vector3f minAabb = stack.allocVector3f();
        Vector3f maxAabb = stack.allocVector3f();
        Vector3f colorvec = stack.allocVector3f();
        // todo: iterate over awake simulation islands!
        for (i = 0; i < collisionObjects.size(); i++) {
            CollisionObject colObj = collisionObjects.getQuick(i);
            if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
                Vector3f color = stack.allocVector3f();
                color.set(255f, 255f, 255f);
                switch(colObj.getActivationState()) {
                    case CollisionObject.ACTIVE_TAG:
                        color.set(255f, 255f, 255f);
                        break;
                    case CollisionObject.ISLAND_SLEEPING:
                        color.set(0f, 255f, 0f);
                        break;
                    case CollisionObject.WANTS_DEACTIVATION:
                        color.set(0f, 255f, 255f);
                        break;
                    case CollisionObject.DISABLE_DEACTIVATION:
                        color.set(255f, 0f, 0f);
                        break;
                    case CollisionObject.DISABLE_SIMULATION:
                        color.set(255f, 255f, 0f);
                        break;
                    default:
                        {
                            color.set(255f, 0f, 0f);
                        }
                }
                debugDrawObject(colObj.getWorldTransform(tmpTrans), colObj.getCollisionShape(), color);
            }
            if (debugDrawer != null && (debugDrawer.getDebugMode() & DebugDrawModes.DRAW_AABB) != 0) {
                colorvec.set(1f, 0f, 0f);
                colObj.getCollisionShape().getAabb(colObj.getWorldTransform(tmpTrans), minAabb, maxAabb);
                debugDrawer.drawAabb(minAabb, maxAabb, colorvec);
            }
        }
        Vector3f wheelColor = stack.allocVector3f();
        Vector3f wheelPosWS = stack.allocVector3f();
        Vector3f axle = stack.allocVector3f();
        Vector3f tmp = stack.allocVector3f();
        for (i = 0; i < vehicles.size(); i++) {
            for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
                wheelColor.set(0, 255, 255);
                if (vehicles.getQuick(i).getWheelInfo(v).raycastInfo.isInContact) {
                    wheelColor.set(0, 0, 255);
                } else {
                    wheelColor.set(255, 0, 255);
                }
                wheelPosWS.set(vehicles.getQuick(i).getWheelInfo(v).worldTransform.origin);
                axle.set(vehicles.getQuick(i).getWheelInfo(v).worldTransform.basis.getElement(0, vehicles.getQuick(i).getRightAxis()), vehicles.getQuick(i).getWheelInfo(v).worldTransform.basis.getElement(1, vehicles.getQuick(i).getRightAxis()), vehicles.getQuick(i).getWheelInfo(v).worldTransform.basis.getElement(2, vehicles.getQuick(i).getRightAxis()));
                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
                //debug wheels (cylinders)
                tmp.add(wheelPosWS, axle);
                debugDrawer.drawLine(wheelPosWS, tmp, wheelColor);
                debugDrawer.drawLine(wheelPosWS, vehicles.getQuick(i).getWheelInfo(v).raycastInfo.contactPointWS, wheelColor);
            }
        }
        if (getDebugDrawer() != null && getDebugDrawer().getDebugMode() != 0) {
            for (i = 0; i < actions.size(); i++) {
                actions.getQuick(i).debugDraw(debugDrawer);
            }
        }
    }
    stack.leave();
}

Example 7 with PersistentManifold

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

the class SimpleDynamicsWorld method stepSimulation.

/**
	 * maxSubSteps/fixedTimeStep for interpolation is currently ignored for SimpleDynamicsWorld, use DiscreteDynamicsWorld instead.
	 */
@Override
public int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) {
    // apply gravity, predict motion
    predictUnconstraintMotion(timeStep);
    DispatcherInfo dispatchInfo = getDispatchInfo();
    dispatchInfo.timeStep = timeStep;
    dispatchInfo.stepCount = 0;
    dispatchInfo.debugDraw = getDebugDrawer();
    // perform collision detection
    performDiscreteCollisionDetection();
    // solve contact constraints
    int numManifolds = dispatcher1.getNumManifolds();
    if (numManifolds != 0) {
        ObjectArrayList<PersistentManifold> manifoldPtr = ((CollisionDispatcher) dispatcher1).getInternalManifoldPointer();
        ContactSolverInfo infoGlobal = new ContactSolverInfo();
        infoGlobal.timeStep = timeStep;
        constraintSolver.prepareSolve(0, numManifolds);
        constraintSolver.solveGroup(null, 0, manifoldPtr, 0, numManifolds, null, 0, 0, infoGlobal, debugDrawer, /*, m_stackAlloc*/
        dispatcher1);
        constraintSolver.allSolved(infoGlobal, debugDrawer);
    }
    // integrate transforms
    integrateTransforms(timeStep);
    updateAabbs();
    synchronizeMotionStates();
    clearForces();
    return 1;
}

Example 8 with PersistentManifold

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

the class KinematicCharacterController method recoverFromPenetration.

protected boolean recoverFromPenetration(CollisionWorld collisionWorld) {
    boolean penetration = false;
    Stack stack = Stack.enter();
    collisionWorld.getDispatcher().dispatchAllCollisionPairs(ghostObject.getOverlappingPairCache(), collisionWorld.getDispatchInfo(), collisionWorld.getDispatcher());
    currentPosition.set(ghostObject.getWorldTransform(stack.allocTransform()).origin);
    float maxPen = 0.0f;
    for (int i = 0; i < ghostObject.getOverlappingPairCache().getNumOverlappingPairs(); i++) {
        manifoldArray.clear();
        BroadphasePair collisionPair = ghostObject.getOverlappingPairCache().getOverlappingPairArray().getQuick(i);
        if (collisionPair.algorithm != null) {
            collisionPair.algorithm.getAllContactManifolds(manifoldArray);
        }
        for (int j = 0; j < manifoldArray.size(); j++) {
            PersistentManifold manifold = manifoldArray.getQuick(j);
            float directionSign = manifold.getBody0() == ghostObject ? -1.0f : 1.0f;
            for (int p = 0; p < manifold.getNumContacts(); p++) {
                ManifoldPoint pt = manifold.getContactPoint(p);
                float dist = pt.getDistance();
                if (dist < 0.0f) {
                    if (dist < maxPen) {
                        maxPen = dist;
                        //??
                        touchingNormal.set(pt.normalWorldOnB);
                        touchingNormal.scale(directionSign);
                    }
                    currentPosition.scaleAdd(directionSign * dist * 0.2f, pt.normalWorldOnB, currentPosition);
                    penetration = true;
                } else {
                //printf("touching %f\n", dist);
                }
            }
        //manifold->clearManifold();
        }
    }
    Transform newTrans = ghostObject.getWorldTransform(stack.allocTransform());
    newTrans.origin.set(currentPosition);
    ghostObject.setWorldTransform(newTrans);
    //printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
    //System.out.println("recoverFromPenetration "+penetration+" "+touchingNormal);
    stack.leave();
    return penetration;
}

Example 9 with PersistentManifold

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

the class SequentialImpulseConstraintSolver method solveGroup.

/**
	 * Sequentially applies impulses.
	 */
@Override
public float solveGroup(ObjectArrayList<CollisionObject> bodies, int numBodies, ObjectArrayList<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, ObjectArrayList<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, Dispatcher dispatcher) {
    BulletStats.pushProfile("solveGroup");
    try {
        // TODO: solver cache friendly
        if ((infoGlobal.solverMode & SolverMode.SOLVER_CACHE_FRIENDLY) != 0) {
            // SimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
            assert (bodies != null);
            assert (numBodies != 0);
            float value = solveGroupCacheFriendly(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer);
            return value;
        }
        ContactSolverInfo info = new ContactSolverInfo(infoGlobal);
        int numiter = infoGlobal.numIterations;
        int totalPoints = 0;
        {
            short j;
            for (j = 0; j < numManifolds; j++) {
                PersistentManifold manifold = manifoldPtr.getQuick(manifold_offset + j);
                prepareConstraints(manifold, info, debugDrawer);
                for (short p = 0; p < manifoldPtr.getQuick(manifold_offset + j).getNumContacts(); p++) {
                    gOrder[totalPoints].manifoldIndex = j;
                    gOrder[totalPoints].pointIndex = p;
                    totalPoints++;
                }
            }
        }
        {
            int j;
            for (j = 0; j < numConstraints; j++) {
                TypedConstraint constraint = constraints.getQuick(constraints_offset + j);
                constraint.buildJacobian();
            }
        }
        // should traverse the contacts random order...
        int iteration;
        {
            for (iteration = 0; iteration < numiter; iteration++) {
                int j;
                if ((infoGlobal.solverMode & SolverMode.SOLVER_RANDMIZE_ORDER) != 0) {
                    if ((iteration & 7) == 0) {
                        for (j = 0; j < totalPoints; ++j) {
                            // JAVA NOTE: swaps references instead of copying values (but that's fine in this context)
                            OrderIndex tmp = gOrder[j];
                            int swapi = randInt2(j + 1);
                            gOrder[j] = gOrder[swapi];
                            gOrder[swapi] = tmp;
                        }
                    }
                }
                for (j = 0; j < numConstraints; j++) {
                    TypedConstraint constraint = constraints.getQuick(constraints_offset + j);
                    constraint.solveConstraint(info.timeStep);
                }
                for (j = 0; j < totalPoints; j++) {
                    PersistentManifold manifold = manifoldPtr.getQuick(manifold_offset + gOrder[j].manifoldIndex);
                    solve((RigidBody) manifold.getBody0(), (RigidBody) manifold.getBody1(), manifold.getContactPoint(gOrder[j].pointIndex), info, iteration, debugDrawer);
                }
                for (j = 0; j < totalPoints; j++) {
                    PersistentManifold manifold = manifoldPtr.getQuick(manifold_offset + gOrder[j].manifoldIndex);
                    solveFriction((RigidBody) manifold.getBody0(), (RigidBody) manifold.getBody1(), manifold.getContactPoint(gOrder[j].pointIndex), info, iteration, debugDrawer);
                }
            }
        }
        return 0f;
    } finally {
        BulletStats.popProfile();
    }
}