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

Example 6 with ManifoldPoint

use of com.bulletphysics.collision.narrowphase.ManifoldPoint in project Terasology by MovingBlocks.

the class BulletPhysics method getNewCollisionPairs.

private Collection<? extends PhysicsSystem.CollisionPair> getNewCollisionPairs() {
    List<PhysicsSystem.CollisionPair> collisionPairs = Lists.newArrayList();
    DynamicsWorld world = discreteDynamicsWorld;
    ObjectArrayList<PersistentManifold> manifolds = new ObjectArrayList<>();
    for (PairCachingGhostObject trigger : entityTriggers.values()) {
        EntityRef entity = (EntityRef) trigger.getUserPointer();
        for (BroadphasePair initialPair : trigger.getOverlappingPairCache().getOverlappingPairArray()) {
            EntityRef otherEntity = null;
            if (initialPair.pProxy0.clientObject == trigger) {
                if (((CollisionObject) initialPair.pProxy1.clientObject).getUserPointer() instanceof EntityRef) {
                    otherEntity = (EntityRef) ((CollisionObject) initialPair.pProxy1.clientObject).getUserPointer();
                }
            } else {
                if (((CollisionObject) initialPair.pProxy0.clientObject).getUserPointer() instanceof EntityRef) {
                    otherEntity = (EntityRef) ((CollisionObject) initialPair.pProxy0.clientObject).getUserPointer();
                }
            }
            if (otherEntity == null || otherEntity == EntityRef.NULL) {
                continue;
            }
            BroadphasePair pair = world.getPairCache().findPair(initialPair.pProxy0, initialPair.pProxy1);
            if (pair == null) {
                continue;
            }
            manifolds.clear();
            if (pair.algorithm != null) {
                pair.algorithm.getAllContactManifolds(manifolds);
            }
            for (PersistentManifold manifold : manifolds) {
                for (int point = 0; point < manifold.getNumContacts(); ++point) {
                    ManifoldPoint manifoldPoint = manifold.getContactPoint(point);
                    if (manifoldPoint.getDistance() < 0) {
                        collisionPairs.add(new PhysicsSystem.CollisionPair(entity, otherEntity, VecMath.from(manifoldPoint.positionWorldOnA), VecMath.from(manifoldPoint.positionWorldOnB), manifoldPoint.getDistance(), VecMath.from(manifoldPoint.normalWorldOnB)));
                        break;
                    }
                }
            }
        }
    }
    return collisionPairs;
}

Example 7 with ManifoldPoint

use of com.bulletphysics.collision.narrowphase.ManifoldPoint 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 8 with ManifoldPoint

use of com.bulletphysics.collision.narrowphase.ManifoldPoint 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 ManifoldPoint

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

the class SequentialImpulseConstraintSolver method prepareConstraints.

protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info, IDebugDraw debugDrawer) {
    Stack stack = Stack.enter();
    RigidBody body0 = (RigidBody) manifoldPtr.getBody0();
    RigidBody body1 = (RigidBody) manifoldPtr.getBody1();
    // only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
    {
        //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
        //manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
        //#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
        int numpoints = manifoldPtr.getNumContacts();
        BulletStats.gTotalContactPoints += numpoints;
        Vector3f tmpVec = stack.allocVector3f();
        Matrix3f tmpMat3 = stack.allocMatrix3f();
        Vector3f pos1 = stack.allocVector3f();
        Vector3f pos2 = stack.allocVector3f();
        Vector3f rel_pos1 = stack.allocVector3f();
        Vector3f rel_pos2 = stack.allocVector3f();
        Vector3f vel1 = stack.allocVector3f();
        Vector3f vel2 = stack.allocVector3f();
        Vector3f vel = stack.allocVector3f();
        Vector3f totalImpulse = stack.allocVector3f();
        Vector3f torqueAxis0 = stack.allocVector3f();
        Vector3f torqueAxis1 = stack.allocVector3f();
        Vector3f ftorqueAxis0 = stack.allocVector3f();
        Vector3f ftorqueAxis1 = stack.allocVector3f();
        for (int i = 0; i < numpoints; i++) {
            ManifoldPoint cp = manifoldPtr.getContactPoint(i);
            if (cp.getDistance() <= 0f) {
                cp.getPositionWorldOnA(pos1);
                cp.getPositionWorldOnB(pos2);
                rel_pos1.sub(pos1, body0.getCenterOfMassPosition(tmpVec));
                rel_pos2.sub(pos2, body1.getCenterOfMassPosition(tmpVec));
                // this jacobian entry is re-used for all iterations
                Matrix3f mat1 = body0.getCenterOfMassTransform(stack.allocTransform()).basis;
                mat1.transpose();
                Matrix3f mat2 = body1.getCenterOfMassTransform(stack.allocTransform()).basis;
                mat2.transpose();
                JacobianEntry jac = jacobiansPool.get();
                jac.init(mat1, mat2, rel_pos1, rel_pos2, cp.normalWorldOnB, body0.getInvInertiaDiagLocal(stack.allocVector3f()), body0.getInvMass(), body1.getInvInertiaDiagLocal(stack.allocVector3f()), body1.getInvMass());
                float jacDiagAB = jac.getDiagonal();
                jacobiansPool.release(jac);
                ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
                if (cpd != null) {
                    // might be invalid
                    cpd.persistentLifeTime++;
                    if (cpd.persistentLifeTime != cp.getLifeTime()) {
                        //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                        //new (cpd) btConstraintPersistentData;
                        cpd.reset();
                        cpd.persistentLifeTime = cp.getLifeTime();
                    } else {
                    //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                    }
                } else {
                    // todo: should this be in a pool?
                    //void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
                    //cpd = new (mem)btConstraintPersistentData;
                    cpd = new ConstraintPersistentData();
                    //assert(cpd != null);
                    totalCpd++;
                    //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                    cp.userPersistentData = cpd;
                    cpd.persistentLifeTime = cp.getLifeTime();
                //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
                }
                assert (cpd != null);
                cpd.jacDiagABInv = 1f / jacDiagAB;
                // Dependent on Rigidbody A and B types, fetch the contact/friction response func
                // perhaps do a similar thing for friction/restutution combiner funcs...
                cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType];
                cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType];
                body0.getVelocityInLocalPoint(rel_pos1, vel1);
                body1.getVelocityInLocalPoint(rel_pos2, vel2);
                vel.sub(vel1, vel2);
                float rel_vel;
                rel_vel = cp.normalWorldOnB.dot(vel);
                float combinedRestitution = cp.combinedRestitution;
                ///btScalar(info.m_numIterations);
                cpd.penetration = cp.getDistance();
                cpd.friction = cp.combinedFriction;
                cpd.restitution = restitutionCurve(rel_vel, combinedRestitution);
                if (cpd.restitution <= 0f) {
                    cpd.restitution = 0f;
                }
                // restitution and penetration work in same direction so
                // rel_vel 
                float penVel = -cpd.penetration / info.timeStep;
                if (cpd.restitution > penVel) {
                    cpd.penetration = 0f;
                }
                float relaxation = info.damping;
                if ((info.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
                    cpd.appliedImpulse *= relaxation;
                } else {
                    cpd.appliedImpulse = 0f;
                }
                // for friction
                cpd.prevAppliedImpulse = cpd.appliedImpulse;
                // re-calculate friction direction every frame, todo: check if this is really needed
                TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1);
                //#define NO_FRICTION_WARMSTART 1
                //#ifdef NO_FRICTION_WARMSTART
                cpd.accumulatedTangentImpulse0 = 0f;
                cpd.accumulatedTangentImpulse1 = 0f;
                //#endif //NO_FRICTION_WARMSTART
                float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0);
                float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0);
                float denom = relaxation / (denom0 + denom1);
                cpd.jacDiagABInvTangent0 = denom;
                denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1);
                denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1);
                denom = relaxation / (denom0 + denom1);
                cpd.jacDiagABInvTangent1 = denom;
                //btVector3 totalImpulse = 
                //	//#ifndef NO_FRICTION_WARMSTART
                //	//cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
                //	//cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
                //	//#endif //NO_FRICTION_WARMSTART
                //	cp.normalWorldOnB*cpd.appliedImpulse;
                totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB);
                ///
                {
                    torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
                    cpd.angularComponentA.set(torqueAxis0);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentA);
                    torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
                    cpd.angularComponentB.set(torqueAxis1);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.angularComponentB);
                }
                {
                    ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
                    cpd.frictionAngularComponent0A.set(ftorqueAxis0);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0A);
                }
                {
                    ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
                    cpd.frictionAngularComponent1A.set(ftorqueAxis1);
                    body0.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1A);
                }
                {
                    ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
                    cpd.frictionAngularComponent0B.set(ftorqueAxis0);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent0B);
                }
                {
                    ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
                    cpd.frictionAngularComponent1B.set(ftorqueAxis1);
                    body1.getInvInertiaTensorWorld(tmpMat3).transform(cpd.frictionAngularComponent1B);
                }
                ///
                // apply previous frames impulse on both bodies
                body0.applyImpulse(totalImpulse, rel_pos1);
                tmpVec.negate(totalImpulse);
                body1.applyImpulse(tmpVec, rel_pos2);
            }
        }
    }
    stack.leave();
}