use of spacegraph.space3d.phys.collision.narrow.ManifoldPoint in project narchy by automenta.
the class SequentialImpulseConstrainer method solveGroupCacheFriendlySetup.
public float solveGroupCacheFriendlySetup(Collection<Collidable> bodies, int numBodies, FasterList<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, FasterList<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal) /*,btStackAlloc* stackAlloc*/
{
if ((numConstraints + numManifolds) == 0) {
// printf("empty\n");
return 0f;
}
PersistentManifold manifold = null;
Collidable 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
// 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;
v3 rel_pos1 = new v3();
v3 rel_pos2 = new v3();
v3 pos1 = new v3();
v3 pos2 = new v3();
v3 vel = new v3();
v3 torqueAxis0 = new v3();
v3 torqueAxis1 = new v3();
v3 vel1 = new v3();
v3 vel2 = new v3();
// Vector3f frictionDir1 = new Vector3f();
// Vector3f frictionDir2 = new Vector3f();
v3 vec = new v3();
Matrix3f tmpMat = new Matrix3f();
for (i = 0; i < numManifolds; i++) {
// return array[index];
manifold = manifoldPtr.get(manifold_offset + i);
colObj0 = (Collidable) manifold.getBody0();
colObj1 = (Collidable) manifold.getBody1();
int solverBodyIdA = -1;
int solverBodyIdB = -1;
if (manifold.numContacts() != 0) {
if (colObj0.tag() >= 0) {
if (colObj0.getCompanionId() >= 0) {
// body has already been converted
solverBodyIdA = colObj0.getCompanionId();
} else {
solverBodyIdA = tmpSolverBodyPool.size();
SolverBody solverBody = new SolverBody();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj0);
colObj0.setCompanionId(solverBodyIdA);
}
} else {
// create a static body
solverBodyIdA = tmpSolverBodyPool.size();
SolverBody solverBody = new SolverBody();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj0);
}
if (colObj1.tag() >= 0) {
if (colObj1.getCompanionId() >= 0) {
solverBodyIdB = colObj1.getCompanionId();
} else {
solverBodyIdB = tmpSolverBodyPool.size();
SolverBody solverBody = new SolverBody();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj1);
colObj1.setCompanionId(solverBodyIdB);
}
} else {
// create a static body
solverBodyIdB = tmpSolverBodyPool.size();
SolverBody solverBody = new SolverBody();
tmpSolverBodyPool.add(solverBody);
initSolverBody(solverBody, colObj1);
}
}
float relaxation;
for (int j = 0; j < manifold.numContacts(); j++) {
ManifoldPoint cp = manifold.getContactPoint(j);
if (cp.distance1 <= 0f) {
cp.getPositionWorldOnA(pos1);
cp.getPositionWorldOnB(pos2);
rel_pos1.sub(pos1, colObj0.transform);
rel_pos2.sub(pos2, colObj1.transform);
relaxation = 1f;
float rel_vel;
int frictionIndex = tmpSolverConstraintPool.size();
SolverConstraint solverConstraint = new SolverConstraint();
tmpSolverConstraintPool.add(solverConstraint);
Body3D rb0 = Body3D.ifDynamic(colObj0);
Body3D rb1 = Body3D.ifDynamic(colObj1);
solverConstraint.solverBodyIdA = solverBodyIdA;
solverConstraint.solverBodyIdB = solverBodyIdB;
solverConstraint.constraintType = SolverConstraint.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.zero();
}
if (rb1 != null) {
rb1.getVelocityInLocalPoint(rel_pos2, vel2);
} else {
vel2.zero();
}
vel.sub(vel1, vel2);
rel_vel = cp.normalWorldOnB.dot(vel);
solverConstraint.penetration = Math.min(cp.distance1 + 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;
}
v3 tmp = new v3();
// 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);
// return array[index];
tmpSolverBodyPool.get(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, solverConstraint.angularComponentA, solverConstraint.appliedImpulse);
}
if (rb1 != null) {
tmp.scale(rb1.getInvMass(), solverConstraint.contactNormal);
// return array[index];
tmpSolverBodyPool.get(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);
}
// return array[index];
SolverConstraint frictionConstraint1 = tmpSolverFrictionConstraintPool.get(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);
// return array[index];
tmpSolverBodyPool.get(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint1.angularComponentA, frictionConstraint1.appliedImpulse);
}
if (rb1 != null) {
tmp.scale(rb1.getInvMass(), frictionConstraint1.contactNormal);
// return array[index];
tmpSolverBodyPool.get(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint1.angularComponentB, -frictionConstraint1.appliedImpulse);
}
} else {
frictionConstraint1.appliedImpulse = 0f;
}
// return array[index];
SolverConstraint frictionConstraint2 = tmpSolverFrictionConstraintPool.get(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);
// return array[index];
tmpSolverBodyPool.get(solverConstraint.solverBodyIdA).internalApplyImpulse(tmp, frictionConstraint2.angularComponentA, frictionConstraint2.appliedImpulse);
}
if (rb1 != null) {
tmp.scale(rb1.getInvMass(), frictionConstraint2.contactNormal);
// return array[index];
tmpSolverBodyPool.get(solverConstraint.solverBodyIdB).internalApplyImpulse(tmp, frictionConstraint2.angularComponentB, -frictionConstraint2.appliedImpulse);
}
} else {
frictionConstraint2.appliedImpulse = 0f;
}
}
}
}
}
// TODO: btContactSolverInfo info = infoGlobal;
int j;
for (j = 0; j < numConstraints; j++) {
constraints.get(constraints_offset + j).buildJacobian();
}
// int j;
// for (j = 0; j < numConstraints; j++) {
// constraints.get(constraints_offset + j).getInfo2(infoGlobal);
// }
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.setBoth(i);
}
for (i = 0; i < numFrictionPool; i++) {
orderFrictionConstraintPool.setBoth(i);
}
return 0f;
}
use of spacegraph.space3d.phys.collision.narrow.ManifoldPoint in project narchy by automenta.
the class ManifoldResult method addContactPoint.
@Override
public void addContactPoint(v3 normalOnBInWorld, v3 pointInWorld, float depth, float breakingThresh) {
assert (manifoldPtr != null);
if (depth > breakingThresh) {
return;
}
boolean isSwapped = manifoldPtr.getBody0() != body0;
v3 pointA = new v3();
pointA.scaleAdd(depth, normalOnBInWorld, pointInWorld);
v3 localA = new v3();
v3 localB = new v3();
if (isSwapped) {
rootTransB.invXform(pointA, localA);
rootTransA.invXform(pointInWorld, localB);
} else {
rootTransA.invXform(pointA, localA);
rootTransB.invXform(pointInWorld, localB);
}
ManifoldPoint newPt = new ManifoldPoint();
newPt.init(localA, localB, normalOnBInWorld, depth);
newPt.positionWorldOnA.set(pointA);
newPt.positionWorldOnB.set(pointInWorld);
int insertIndex = manifoldPtr.getCacheEntry(newPt, manifoldPtr.getContactBreakingThreshold());
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 (manifoldPtr.globals.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.
Collidable obj0 = isSwapped ? body1 : body0;
Collidable obj1 = isSwapped ? body0 : body1;
manifoldPtr.globals.getContactAddedCallback().contactAdded(manifoldPtr.getContactPoint(insertIndex), obj0, partId0, index0, obj1, partId1, index1);
}
}
use of spacegraph.space3d.phys.collision.narrow.ManifoldPoint in project narchy by automenta.
the class SequentialImpulseConstrainer method solveGroupCacheFriendly.
float solveGroupCacheFriendly(Collection<Collidable> bodies, int numBodies, FasterList<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, FasterList<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal) /*,btStackAlloc* stackAlloc*/
{
solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal);
int numPoolConstraints = tmpSolverConstraintPool.size();
for (int j = 0; j < numPoolConstraints; j++) {
// return array[index];
SolverConstraint solveManifold = tmpSolverConstraintPool.get(j);
ManifoldPoint pt = (ManifoldPoint) solveManifold.originalContactPoint;
assert (pt != null);
pt.appliedImpulse = solveManifold.appliedImpulse;
// return array[index];
pt.appliedImpulseLateral1 = tmpSolverFrictionConstraintPool.get(solveManifold.frictionIndex).appliedImpulse;
// return array[index];
pt.appliedImpulseLateral1 = tmpSolverFrictionConstraintPool.get(solveManifold.frictionIndex + 1).appliedImpulse;
// do a callback here?
}
if (infoGlobal.splitImpulse) {
for (int i = 0; i < tmpSolverBodyPool.size(); i++) {
// return array[index];
tmpSolverBodyPool.get(i).writebackVelocity(infoGlobal.timeStep);
}
} else {
for (int i = 0; i < tmpSolverBodyPool.size(); i++) {
// return array[index];
tmpSolverBodyPool.get(i).writebackVelocity();
}
}
// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
/*
printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
*/
tmpSolverBodyPool.clearFast();
tmpSolverConstraintPool.clearFast();
tmpSolverFrictionConstraintPool.clearFast();
return 0f;
}
use of spacegraph.space3d.phys.collision.narrow.ManifoldPoint in project narchy by automenta.
the class SequentialImpulseConstrainer method prepareConstraints.
protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info) {
Body3D body0 = (Body3D) manifoldPtr.getBody0();
Body3D body1 = (Body3D) 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.numContacts();
BulletStats.gTotalContactPoints += numpoints;
v3 tmpVec = new v3();
v3 pos1 = new v3();
v3 pos2 = new v3();
v3 rel_pos1 = new v3();
v3 rel_pos2 = new v3();
v3 vel1 = new v3();
v3 vel2 = new v3();
v3 vel = new v3();
v3 totalImpulse = new v3();
v3 torqueAxis0 = new v3();
v3 torqueAxis1 = new v3();
v3 ftorqueAxis0 = new v3();
v3 ftorqueAxis1 = new v3();
final Transform tt1 = new Transform(), tt2 = new Transform();
JacobianEntry jac = new JacobianEntry();
for (int i = 0; i < numpoints; i++) {
ManifoldPoint cp = manifoldPtr.getContactPoint(i);
if (cp.distance1 <= 0f) {
cp.getPositionWorldOnA(pos1);
cp.getPositionWorldOnB(pos2);
rel_pos1.sub(pos1, body0.transform);
rel_pos2.sub(pos2, body1.transform);
// this jacobian entry is re-used for all iterations
Matrix3f mat1 = body0.transform.basis;
mat1.transpose();
Matrix3f mat2 = body1.transform.basis;
mat2.transpose();
jac.init(mat1, mat2, rel_pos1, rel_pos2, cp.normalWorldOnB, body0.invInertiaLocal, body0.getInvMass(), body1.invInertiaLocal, body1.getInvMass());
float jacDiagAB = jac.Adiag;
ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
if (cpd != null) {
// might be invalid
cpd.persistentLifeTime++;
if (cpd.persistentLifeTime != cp.lifeTime) {
// printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
// new (cpd) btConstraintPersistentData;
cpd.reset();
cpd.persistentLifeTime = cp.lifeTime;
} 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.lifeTime;
// 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.distance1;
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.invInertiaTensorWorld.transform(cpd.angularComponentA);
torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
cpd.angularComponentB.set(torqueAxis1);
body1.invInertiaTensorWorld.transform(cpd.angularComponentB);
ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
cpd.frictionAngularComponent0A.set(ftorqueAxis0);
body0.invInertiaTensorWorld.transform(cpd.frictionAngularComponent0A);
ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
cpd.frictionAngularComponent1A.set(ftorqueAxis1);
body0.invInertiaTensorWorld.transform(cpd.frictionAngularComponent1A);
ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
cpd.frictionAngularComponent0B.set(ftorqueAxis0);
body1.invInertiaTensorWorld.transform(cpd.frictionAngularComponent0B);
ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
cpd.frictionAngularComponent1B.set(ftorqueAxis1);
body1.invInertiaTensorWorld.transform(cpd.frictionAngularComponent1B);
// /
// apply previous frames impulse on both bodies
body0.impulse(totalImpulse, rel_pos1);
tmpVec.negate(totalImpulse);
body1.impulse(tmpVec, rel_pos2);
}
}
}
Aggregations