use of com.bulletphysics.collision.narrowphase.PersistentManifold in project bdx by GoranM.
the class CollisionDispatcher method getNewManifold.
@Override
public PersistentManifold getNewManifold(Object b0, Object b1) {
//gNumManifold++;
//btAssert(gNumManifold < 65535);
CollisionObject body0 = (CollisionObject) b0;
CollisionObject body1 = (CollisionObject) b1;
/*
void* mem = 0;
if (m_persistentManifoldPoolAllocator->getFreeCount())
{
mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
} else
{
mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
}
btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back(manifold);
*/
PersistentManifold manifold = manifoldsPool.get();
manifold.init(body0, body1, 0);
manifold.index1a = manifoldsPtr.size();
manifoldsPtr.add(manifold);
return manifold;
}
use of com.bulletphysics.collision.narrowphase.PersistentManifold in project bdx by GoranM.
the class SequentialImpulseConstraintSolver method solveGroupCacheFriendlySetup.
public float solveGroupCacheFriendlySetup(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) /*,btStackAlloc* stackAlloc*/
{
BulletStats.pushProfile("solveGroupCacheFriendlySetup");
Stack stack = Stack.enter();
int sp = stack.getSp();
try {
if ((numConstraints + numManifolds) == 0) {
// printf("empty\n");
return 0f;
}
PersistentManifold manifold = null;
CollisionObject colObj0 = null, colObj1 = null;
//btRigidBody* rb0=0,*rb1=0;
// //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
//
// BEGIN_PROFILE("refreshManifolds");
//
// int i;
//
//
//
// for (i=0;i<numManifolds;i++)
// {
// manifold = manifoldPtr[i];
// rb1 = (btRigidBody*)manifold->getBody1();
// rb0 = (btRigidBody*)manifold->getBody0();
//
// manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
//
// }
//
// END_PROFILE("refreshManifolds");
// //#endif //FORCE_REFESH_CONTACT_MANIFOLDS
Transform tmpTrans = stack.allocTransform();
//int sizeofSB = sizeof(btSolverBody);
//int sizeofSC = sizeof(btSolverConstraint);
//if (1)
{
//if m_stackAlloc, try to pack bodies/constraints to speed up solving
// btBlock* sablock;
// sablock = stackAlloc->beginBlock();
// int memsize = 16;
// unsigned char* stackMemory = stackAlloc->allocate(memsize);
// todo: use stack allocator for this temp memory
//int minReservation = numManifolds * 2;
//m_tmpSolverBodyPool.reserve(minReservation);
//don't convert all bodies, only the one we need so solver the constraints
/*
{
for (int i=0;i<numBodies;i++)
{
btRigidBody* rb = btRigidBody::upcast(bodies[i]);
if (rb && (rb->getIslandTag() >= 0))
{
btAssert(rb->getCompanionId() < 0);
int solverBodyId = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb);
rb->setCompanionId(solverBodyId);
}
}
}
*/
//m_tmpSolverConstraintPool.reserve(minReservation);
//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
{
int i;
Vector3f rel_pos1 = stack.allocVector3f();
Vector3f rel_pos2 = stack.allocVector3f();
Vector3f pos1 = stack.allocVector3f();
Vector3f pos2 = stack.allocVector3f();
Vector3f vel = stack.allocVector3f();
Vector3f torqueAxis0 = stack.allocVector3f();
Vector3f torqueAxis1 = stack.allocVector3f();
Vector3f vel1 = stack.allocVector3f();
Vector3f vel2 = stack.allocVector3f();
Vector3f frictionDir1 = stack.allocVector3f();
Vector3f frictionDir2 = stack.allocVector3f();
Vector3f vec = stack.allocVector3f();
Matrix3f tmpMat = stack.allocMatrix3f();
for (i = 0; i < numManifolds; i++) {
manifold = manifoldPtr.getQuick(manifold_offset + i);
colObj0 = (CollisionObject) manifold.getBody0();
colObj1 = (CollisionObject) manifold.getBody1();
int solverBodyIdA = -1;
int solverBodyIdB = -1;
if (manifold.getNumContacts() != 0) {
if (colObj0.getIslandTag() >= 0) {
if (colObj0.getCompanionId() >= 0) {
// body has already been converted
solverBodyIdA = colObj0.getCompanionId();
} else {
solverBodyIdA = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj0);
colObj0.setCompanionId(solverBodyIdA);
}
} else {
// create a static body
solverBodyIdA = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj0);
}
if (colObj1.getIslandTag() >= 0) {
if (colObj1.getCompanionId() >= 0) {
solverBodyIdB = colObj1.getCompanionId();
} else {
solverBodyIdB = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj1);
colObj1.setCompanionId(solverBodyIdB);
}
} else {
// create a static body
solverBodyIdB = tmpSolverBodyPool.size();
SolverBody solverBody = bodiesPool.get();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj1);
}
}
float relaxation;
for (int j = 0; j < manifold.getNumContacts(); j++) {
ManifoldPoint cp = manifold.getContactPoint(j);
if (cp.getDistance() <= 0f) {
cp.getPositionWorldOnA(pos1);
cp.getPositionWorldOnB(pos2);
rel_pos1.sub(pos1, colObj0.getWorldTransform(tmpTrans).origin);
rel_pos2.sub(pos2, colObj1.getWorldTransform(tmpTrans).origin);
relaxation = 1f;
float rel_vel;
int frictionIndex = tmpSolverConstraintPool.size();
{
SolverConstraint solverConstraint = constraintsPool.get();
tmpSolverConstraintPool.add(solverConstraint);
RigidBody rb0 = RigidBody.upcast(colObj0);
RigidBody rb1 = RigidBody.upcast(colObj1);
solverConstraint.solverBodyIdA = solverBodyIdA;
solverConstraint.solverBodyIdB = solverBodyIdB;
solverConstraint.constraintType = SolverConstraintType.SOLVER_CONTACT_1D;
solverConstraint.originalContactPoint = cp;
torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
if (rb0 != null) {
solverConstraint.angularComponentA.set(torqueAxis0);
rb0.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentA);
} else {
solverConstraint.angularComponentA.set(0f, 0f, 0f);
}
torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
if (rb1 != null) {
solverConstraint.angularComponentB.set(torqueAxis1);
rb1.getInvInertiaTensorWorld(tmpMat).transform(solverConstraint.angularComponentB);
} else {
solverConstraint.angularComponentB.set(0f, 0f, 0f);
}
{
//#ifdef COMPUTE_IMPULSE_DENOM
//btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
//btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
//#else
float denom0 = 0f;
float denom1 = 0f;
if (rb0 != null) {
vec.cross(solverConstraint.angularComponentA, rel_pos1);
denom0 = rb0.getInvMass() + cp.normalWorldOnB.dot(vec);
}
if (rb1 != null) {
vec.cross(solverConstraint.angularComponentB, rel_pos2);
denom1 = rb1.getInvMass() + cp.normalWorldOnB.dot(vec);
}
//#endif //COMPUTE_IMPULSE_DENOM
float denom = relaxation / (denom0 + denom1);
solverConstraint.jacDiagABInv = denom;
}
solverConstraint.contactNormal.set(cp.normalWorldOnB);
solverConstraint.relpos1CrossNormal.cross(rel_pos1, cp.normalWorldOnB);
solverConstraint.relpos2CrossNormal.cross(rel_pos2, cp.normalWorldOnB);
if (rb0 != null) {
rb0.getVelocityInLocalPoint(rel_pos1, vel1);
} else {
vel1.set(0f, 0f, 0f);
}
if (rb1 != null) {
rb1.getVelocityInLocalPoint(rel_pos2, vel2);
} else {
vel2.set(0f, 0f, 0f);
}
vel.sub(vel1, vel2);
rel_vel = cp.normalWorldOnB.dot(vel);
solverConstraint.penetration = Math.min(cp.getDistance() + infoGlobal.linearSlop, 0f);
//solverConstraint.m_penetration = cp.getDistance();
solverConstraint.friction = cp.combinedFriction;
solverConstraint.restitution = restitutionCurve(rel_vel, cp.combinedRestitution);
if (solverConstraint.restitution <= 0f) {
solverConstraint.restitution = 0f;
}
float penVel = -solverConstraint.penetration / infoGlobal.timeStep;
if (solverConstraint.restitution > penVel) {
solverConstraint.penetration = 0f;
}
Vector3f tmp = stack.allocVector3f();
// warm starting (or zero if disabled)
if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
solverConstraint.appliedImpulse = cp.appliedImpulse * infoGlobal.warmstartingFactor;
if (rb0 != null) {
tmp.scale(rb0.getInvMass(), solverConstraint.contactNormal);
tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, solverConstraint.angularComponentA, solverConstraint.appliedImpulse);
}
if (rb1 != null) {
tmp.scale(rb1.getInvMass(), solverConstraint.contactNormal);
tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, solverConstraint.angularComponentB, -solverConstraint.appliedImpulse);
}
} else {
solverConstraint.appliedImpulse = 0f;
}
solverConstraint.appliedPushImpulse = 0f;
solverConstraint.frictionIndex = tmpSolverFrictionConstraintPool.size();
if (!cp.lateralFrictionInitialized) {
cp.lateralFrictionDir1.scale(rel_vel, cp.normalWorldOnB);
cp.lateralFrictionDir1.sub(vel, cp.lateralFrictionDir1);
float lat_rel_vel = cp.lateralFrictionDir1.lengthSquared();
if (//0.0f)
lat_rel_vel > BulletGlobals.FLT_EPSILON) {
cp.lateralFrictionDir1.scale(1f / (float) Math.sqrt(lat_rel_vel));
addFrictionConstraint(cp.lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
cp.lateralFrictionDir2.cross(cp.lateralFrictionDir1, cp.normalWorldOnB);
//??
cp.lateralFrictionDir2.normalize();
addFrictionConstraint(cp.lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
} else {
// re-calculate friction direction every frame, todo: check if this is really needed
TransformUtil.planeSpace1(cp.normalWorldOnB, cp.lateralFrictionDir1, cp.lateralFrictionDir2);
addFrictionConstraint(cp.lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
addFrictionConstraint(cp.lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
}
cp.lateralFrictionInitialized = true;
} else {
addFrictionConstraint(cp.lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
addFrictionConstraint(cp.lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
}
{
SolverConstraint frictionConstraint1 = tmpSolverFrictionConstraintPool.getQuick(solverConstraint.frictionIndex);
if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
frictionConstraint1.appliedImpulse = cp.appliedImpulseLateral1 * infoGlobal.warmstartingFactor;
if (rb0 != null) {
tmp.scale(rb0.getInvMass(), frictionConstraint1.contactNormal);
tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint1.angularComponentA, frictionConstraint1.appliedImpulse);
}
if (rb1 != null) {
tmp.scale(rb1.getInvMass(), frictionConstraint1.contactNormal);
tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint1.angularComponentB, -frictionConstraint1.appliedImpulse);
}
} else {
frictionConstraint1.appliedImpulse = 0f;
}
}
{
SolverConstraint frictionConstraint2 = tmpSolverFrictionConstraintPool.getQuick(solverConstraint.frictionIndex + 1);
if ((infoGlobal.solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
frictionConstraint2.appliedImpulse = cp.appliedImpulseLateral2 * infoGlobal.warmstartingFactor;
if (rb0 != null) {
tmp.scale(rb0.getInvMass(), frictionConstraint2.contactNormal);
tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint2.angularComponentA, frictionConstraint2.appliedImpulse);
}
if (rb1 != null) {
tmp.scale(rb1.getInvMass(), frictionConstraint2.contactNormal);
tmpSolverBodyPool.getQuick(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint2.angularComponentB, -frictionConstraint2.appliedImpulse);
}
} else {
frictionConstraint2.appliedImpulse = 0f;
}
}
}
}
}
}
}
}
// TODO: btContactSolverInfo info = infoGlobal;
{
int j;
for (j = 0; j < numConstraints; j++) {
TypedConstraint constraint = constraints.getQuick(constraints_offset + j);
constraint.buildJacobian();
}
}
int numConstraintPool = tmpSolverConstraintPool.size();
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
// todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
MiscUtil.resize(orderTmpConstraintPool, numConstraintPool, 0);
MiscUtil.resize(orderFrictionConstraintPool, numFrictionPool, 0);
{
int i;
for (i = 0; i < numConstraintPool; i++) {
orderTmpConstraintPool.set(i, i);
}
for (i = 0; i < numFrictionPool; i++) {
orderFrictionConstraintPool.set(i, i);
}
}
return 0f;
} finally {
stack.leave(sp);
BulletStats.popProfile();
}
}
use of com.bulletphysics.collision.narrowphase.PersistentManifold in project bdx by GoranM.
the class GameObject method reactionForce.
public float reactionForce() {
float force = 0;
int totalContacts = 0;
for (PersistentManifold m : contactManifolds) {
int numContacts = m.getNumContacts();
totalContacts += numContacts;
for (int i = 0; i < numContacts; ++i) {
ManifoldPoint p = m.getContactPoint(i);
force += p.appliedImpulse;
}
}
return totalContacts != 0 ? force / totalContacts : 0;
}
use of com.bulletphysics.collision.narrowphase.PersistentManifold in project bdx by GoranM.
the class Scene method detectCollisions.
private void detectCollisions() {
for (GameObject g : objects) {
ArrayListGameObject hitLast = g.touchingObjectsLast;
g.touchingObjectsLast = g.touchingObjects;
g.touchingObjects = hitLast;
g.touchingObjects.clear();
g.contactManifolds.clear();
}
int numManifolds = world.getDispatcher().getNumManifolds();
for (int i = 0; i < numManifolds; ++i) {
PersistentManifold mani = world.getDispatcher().getManifoldByIndexInternal(i);
if (mani.getNumContacts() > 0) {
RigidBody a = (RigidBody) mani.getBody0();
RigidBody b = (RigidBody) mani.getBody1();
GameObject A = (GameObject) a.getUserPointer();
GameObject B = (GameObject) b.getUserPointer();
A.touchingObjects.add(B);
B.touchingObjects.add(A);
A.contactManifolds.add(mani);
B.contactManifolds.add(mani);
}
}
}
use of com.bulletphysics.collision.narrowphase.PersistentManifold in project bdx by GoranM.
the class SimulationIslandManager method buildIslands.
public void buildIslands(Dispatcher dispatcher, ObjectArrayList<CollisionObject> collisionObjects) {
BulletStats.pushProfile("islandUnionFindAndQuickSort");
try {
islandmanifold.clear();
// we are going to sort the unionfind array, and store the element id in the size
// afterwards, we clean unionfind, to make sure no-one uses it anymore
getUnionFind().sortIslands();
int numElem = getUnionFind().getNumElements();
int endIslandIndex = 1;
int startIslandIndex;
// update the sleeping state for bodies, if all are sleeping
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) {
int islandId = getUnionFind().getElement(startIslandIndex).id;
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) {
}
//int numSleeping = 0;
boolean allSleeping = true;
int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
int i = getUnionFind().getElement(idx).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
//System.err.println("error in island management\n");
}
assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
if (colObj0.getIslandTag() == islandId) {
if (colObj0.getActivationState() == CollisionObject.ACTIVE_TAG) {
allSleeping = false;
}
if (colObj0.getActivationState() == CollisionObject.DISABLE_DEACTIVATION) {
allSleeping = false;
}
}
}
if (allSleeping) {
//int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
int i = getUnionFind().getElement(idx).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
//System.err.println("error in island management\n");
}
assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
if (colObj0.getIslandTag() == islandId) {
colObj0.setActivationState(CollisionObject.ISLAND_SLEEPING);
}
}
} else {
//int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
int i = getUnionFind().getElement(idx).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
//System.err.println("error in island management\n");
}
assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
if (colObj0.getIslandTag() == islandId) {
if (colObj0.getActivationState() == CollisionObject.ISLAND_SLEEPING) {
colObj0.setActivationState(CollisionObject.WANTS_DEACTIVATION);
}
}
}
}
}
int i;
int maxNumManifolds = dispatcher.getNumManifolds();
for (i = 0; i < maxNumManifolds; i++) {
PersistentManifold manifold = dispatcher.getManifoldByIndexInternal(i);
CollisionObject colObj0 = (CollisionObject) manifold.getBody0();
CollisionObject colObj1 = (CollisionObject) manifold.getBody1();
// todo: check sleeping conditions!
if (((colObj0 != null) && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) || ((colObj1 != null) && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING)) {
// kinematic objects don't merge islands, but wake up all connected objects
if (colObj0.isKinematicObject() && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
colObj1.activate();
}
if (colObj1.isKinematicObject() && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
colObj0.activate();
}
//filtering for response
if (dispatcher.needsResponse(colObj0, colObj1)) {
islandmanifold.add(manifold);
}
//#endif //SPLIT_ISLANDS
}
}
} finally {
BulletStats.popProfile();
}
}
Aggregations