public Vector3f GetCoordinates(Face face, Vector3f out) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f o = Stack.alloc(Vector3f.class);
o.scale(-face.d, face.n);
float[] a = floatArrays.getFixed(3);
tmp1.sub(face.v[0].w, o);
tmp2.sub(face.v[1].w, o);
tmp.cross(tmp1, tmp2);
a[0] = tmp.length();
tmp1.sub(face.v[1].w, o);
tmp2.sub(face.v[2].w, o);
tmp.cross(tmp1, tmp2);
a[1] = tmp.length();
tmp1.sub(face.v[2].w, o);
tmp2.sub(face.v[0].w, o);
tmp.cross(tmp1, tmp2);
a[2] = tmp.length();
float sm = a[0] + a[1] + a[2];
out.set(a[1], a[2], a[0]);
out.scale(1f / (sm > 0f ? sm : 1f));
floatArrays.release(a);
return out;
}
public boolean Set(Face f, Mkv a, Mkv b, Mkv c) {
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
Vector3f nrm = Stack.alloc(Vector3f.class);
tmp1.sub(b.w, a.w);
tmp2.sub(c.w, a.w);
nrm.cross(tmp1, tmp2);
float len = nrm.length();
tmp1.cross(a.w, b.w);
tmp2.cross(b.w, c.w);
tmp3.cross(c.w, a.w);
boolean valid =
(tmp1.dot(nrm) >= -EPA_inface_eps)
&& (tmp2.dot(nrm) >= -EPA_inface_eps)
&& (tmp3.dot(nrm) >= -EPA_inface_eps);
f.v[0] = a;
f.v[1] = b;
f.v[2] = c;
f.mark = 0;
f.n.scale(1f / (len > 0f ? len : cstInf), nrm);
f.d = Math.max(0, -f.n.dot(a.w));
return valid;
}
@Override
public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0, Vector3f out) {
Vector3f supVec = out;
supVec.set(0f, 0f, 0f);
float newDot, maxDot = -1e30f;
Vector3f vec = Stack.alloc(vec0);
float lenSqr = vec.lengthSquared();
if (lenSqr < 0.0001f) {
vec.set(1f, 0f, 0f);
} else {
float rlen = 1f / (float) Math.sqrt(lenSqr);
vec.scale(rlen);
}
Vector3f vtx = Stack.alloc(Vector3f.class);
for (int i = 0; i < points.size(); i++) {
VectorUtil.mul(vtx, points.getQuick(i), localScaling);
newDot = vec.dot(vtx);
if (newDot > maxDot) {
maxDot = newDot;
supVec.set(vtx);
}
}
return out;
}
public boolean SearchOrigin(Vector3f initray) {
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
Vector3f tmp4 = Stack.alloc(Vector3f.class);
iterations = 0;
order = -1;
failed = false;
ray.set(initray);
ray.normalize();
Arrays.fill(table, null);
FetchSupport();
ray.negate(simplex[0].w);
for (; iterations < GJK_maxiterations; ++iterations) {
float rl = ray.length();
ray.scale(1f / (rl > 0f ? rl : 1f));
if (FetchSupport()) {
boolean found = false;
switch (order) {
case 1:
{
tmp1.negate(simplex[1].w);
tmp2.sub(simplex[0].w, simplex[1].w);
found = SolveSimplex2(tmp1, tmp2);
break;
}
case 2:
{
tmp1.negate(simplex[2].w);
tmp2.sub(simplex[1].w, simplex[2].w);
tmp3.sub(simplex[0].w, simplex[2].w);
found = SolveSimplex3(tmp1, tmp2, tmp3);
break;
}
case 3:
{
tmp1.negate(simplex[3].w);
tmp2.sub(simplex[2].w, simplex[3].w);
tmp3.sub(simplex[1].w, simplex[3].w);
tmp4.sub(simplex[0].w, simplex[3].w);
found = SolveSimplex4(tmp1, tmp2, tmp3, tmp4);
break;
}
}
if (found) {
return true;
}
} else {
return false;
}
}
failed = true;
return false;
}
protected void integrateTransforms(float timeStep) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Transform tmpTrans = Stack.alloc(Transform.class);
Transform predictedTrans = Stack.alloc(Transform.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.setHitFraction(1f);
if (body.isActive() && (!body.isStaticOrKinematicObject())) {
body.predictIntegratedTransform(timeStep, predictedTrans);
tmp.sub(predictedTrans.origin, body.getWorldTransform(tmpTrans).origin);
float squareMotion = tmp.lengthSquared();
if (body.getCcdSquareMotionThreshold() != 0f
&& body.getCcdSquareMotionThreshold() < squareMotion) {
if (body.getCollisionShape().isConvex()) {
ClosestNotMeConvexResultCallback sweepResults =
new ClosestNotMeConvexResultCallback(
body,
body.getWorldTransform(tmpTrans).origin,
predictedTrans.origin,
getBroadphase().getOverlappingPairCache(),
getDispatcher());
// ConvexShape convexShape = (ConvexShape)body.getCollisionShape();
SphereShape tmpSphere =
new SphereShape(
body
.getCcdSweptSphereRadius()); // btConvexShape* convexShape =
// static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.collisionFilterGroup = body.getBroadphaseProxy().collisionFilterGroup;
sweepResults.collisionFilterMask = body.getBroadphaseProxy().collisionFilterMask;
convexSweepTest(
tmpSphere, body.getWorldTransform(tmpTrans), predictedTrans, sweepResults);
// JAVA NOTE: added closestHitFraction test to prevent objects being stuck
if (sweepResults.hasHit() && (sweepResults.closestHitFraction > 0.0001f)) {
body.setHitFraction(sweepResults.closestHitFraction);
body.predictIntegratedTransform(timeStep * body.getHitFraction(), predictedTrans);
body.setHitFraction(0f);
// System.out.printf("clamped integration to hit fraction = %f\n",
// sweepResults.closestHitFraction);
}
}
}
body.proceedToTransform(predictedTrans);
}
}
}
}
public void Support(Vector3f d, Mkv v) {
v.r.set(d);
Vector3f tmp1 = LocalSupport(d, 0, Stack.alloc(Vector3f.class));
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.set(d);
tmp.negate();
Vector3f tmp2 = LocalSupport(tmp, 1, Stack.alloc(Vector3f.class));
v.w.sub(tmp1, tmp2);
v.w.scaleAdd(margin, d, v.w);
}
@Override
public void batchedUnitVectorGetSupportingVertexWithoutMargin(
Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
float newDot;
// JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3
// TODO: optimize it
float[] wcoords = new float[numVectors];
// use 'w' component of supportVerticesOut?
{
for (int i = 0; i < numVectors; i++) {
// supportVerticesOut[i][3] = btScalar(-1e30);
wcoords[i] = -1e30f;
}
}
Vector3f vtx = Stack.alloc(Vector3f.class);
for (int i = 0; i < points.size(); i++) {
VectorUtil.mul(vtx, points.getQuick(i), localScaling);
for (int j = 0; j < numVectors; j++) {
Vector3f vec = vectors[j];
newDot = vec.dot(vtx);
// if (newDot > supportVerticesOut[j][3])
if (newDot > wcoords[j]) {
// WARNING: don't swap next lines, the w component would get overwritten!
supportVerticesOut[j].set(vtx);
// supportVerticesOut[j][3] = newDot;
wcoords[j] = newDot;
}
}
}
}
protected void updateActivationState(float timeStep) {
Vector3f tmp = Stack.alloc(Vector3f.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.updateDeactivation(timeStep);
if (body.wantsSleeping()) {
if (body.isStaticOrKinematicObject()) {
body.setActivationState(CollisionObject.ISLAND_SLEEPING);
} else {
if (body.getActivationState() == CollisionObject.ACTIVE_TAG) {
body.setActivationState(CollisionObject.WANTS_DEACTIVATION);
}
if (body.getActivationState() == CollisionObject.ISLAND_SLEEPING) {
tmp.set(0f, 0f, 0f);
body.setAngularVelocity(tmp);
body.setLinearVelocity(tmp);
}
}
} else {
if (body.getActivationState() != CollisionObject.DISABLE_DEACTIVATION) {
body.setActivationState(CollisionObject.ACTIVE_TAG);
}
}
}
}
}
public void awakenRigidBodiesInArea(Vector3f min, Vector3f max) {
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject collisionObject = collisionObjects.getQuick(i);
if (!collisionObject.isStaticOrKinematicObject() && !collisionObject.isActive()) {
Vector3f otherMin = Stack.alloc(Vector3f.class);
Vector3f otherMax = Stack.alloc(Vector3f.class);
collisionObject
.getCollisionShape()
.getAabb(
collisionObject.getWorldTransform(Stack.alloc(Transform.class)),
otherMin,
otherMax);
if (AabbUtil2.testAabbAgainstAabb2(min, max, otherMin, otherMax)) {
collisionObject.activate();
}
}
}
}
public Vector3f LocalSupport(Vector3f d, /*unsigned*/ int i, Vector3f out) {
Vector3f tmp = Stack.alloc(Vector3f.class);
MatrixUtil.transposeTransform(tmp, d, wrotations[i]);
shapes[i].localGetSupportingVertex(tmp, out);
wrotations[i].transform(out);
out.add(positions[i]);
return out;
}
@Override
public float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace) {
if (convexResult.hitCollisionObject == me) {
return 1f;
}
Vector3f linVelA = Stack.alloc(Vector3f.class), linVelB = Stack.alloc(Vector3f.class);
linVelA.sub(convexToWorld, convexFromWorld);
linVelB.set(0f, 0f, 0f); // toB.getOrigin()-fromB.getOrigin();
Vector3f relativeVelocity = Stack.alloc(Vector3f.class);
relativeVelocity.sub(linVelA, linVelB);
// don't report time of impact for motion away from the contact normal (or causes minor
// penetration)
if (convexResult.hitNormalLocal.dot(relativeVelocity) >= -allowedPenetration) {
return 1f;
}
return super.addSingleResult(convexResult, normalInWorldSpace);
}
public void internalProcessAllTriangles(
InternalTriangleIndexCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
int graphicssubparts = getNumSubParts();
Vector3f[] triangle /*[3]*/ =
new Vector3f[] {
Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class)
};
Vector3f meshScaling = getScaling(Stack.alloc(Vector3f.class));
for (int part = 0; part < graphicssubparts; part++) {
VertexData data = getLockedReadOnlyVertexIndexBase(part);
for (int i = 0, cnt = data.getIndexCount() / 3; i < cnt; i++) {
data.getTriangle(i * 3, meshScaling, triangle);
callback.internalProcessTriangleIndex(triangle, part, i);
}
unLockReadOnlyVertexBase(part);
}
}
protected void synchronizeMotionStates() {
Transform interpolatedTransform = Stack.alloc(Transform.class);
Transform tmpTrans = Stack.alloc(Transform.class);
Vector3f tmpLinVel = Stack.alloc(Vector3f.class);
Vector3f tmpAngVel = Stack.alloc(Vector3f.class);
// todo: iterate over awake simulation islands!
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null && body.getMotionState() != null && !body.isStaticOrKinematicObject()) {
// we need to call the update at least once, even for sleeping objects
// otherwise the 'graphics' transform never updates properly
// so todo: add 'dirty' flag
// if (body->getActivationState() != ISLAND_SLEEPING)
{
TransformUtil.integrateTransform(
body.getInterpolationWorldTransform(tmpTrans),
body.getInterpolationLinearVelocity(tmpLinVel),
body.getInterpolationAngularVelocity(tmpAngVel),
localTime * body.getHitFraction(),
interpolatedTransform);
body.getMotionState().setWorldTransform(interpolatedTransform);
}
}
}
if (getDebugDrawer() != null
&& (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
for (int i = 0; i < vehicles.size(); i++) {
for (int v = 0; v < vehicles.getQuick(i).getNumWheels(); v++) {
// synchronize the wheels with the (interpolated) chassis worldtransform
vehicles.getQuick(i).updateWheelTransform(v, true);
}
}
}
}
protected void debugDrawSphere(float radius, Transform transform, Vector3f color) {
Vector3f start = Stack.alloc(transform.origin);
Vector3f xoffs = Stack.alloc(Vector3f.class);
xoffs.set(radius, 0, 0);
transform.basis.transform(xoffs);
Vector3f yoffs = Stack.alloc(Vector3f.class);
yoffs.set(0, radius, 0);
transform.basis.transform(yoffs);
Vector3f zoffs = Stack.alloc(Vector3f.class);
zoffs.set(0, 0, radius);
transform.basis.transform(zoffs);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
// XY
tmp1.sub(start, xoffs);
tmp2.add(start, yoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.add(start, yoffs);
tmp2.add(start, xoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.add(start, xoffs);
tmp2.sub(start, yoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.sub(start, yoffs);
tmp2.sub(start, xoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
// XZ
tmp1.sub(start, xoffs);
tmp2.add(start, zoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.add(start, zoffs);
tmp2.add(start, xoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.add(start, xoffs);
tmp2.sub(start, zoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.sub(start, zoffs);
tmp2.sub(start, xoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
// YZ
tmp1.sub(start, yoffs);
tmp2.add(start, zoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.add(start, zoffs);
tmp2.add(start, yoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.add(start, yoffs);
tmp2.sub(start, zoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
tmp1.sub(start, zoffs);
tmp2.sub(start, yoffs);
getDebugDrawer().drawLine(tmp1, tmp2, color);
}
@Override
public Vector3f localGetSupportingVertex(Vector3f vec, Vector3f out) {
Vector3f supVertex = localGetSupportingVertexWithoutMargin(vec, out);
if (getMargin() != 0f) {
Vector3f vecnorm = Stack.alloc(vec);
if (vecnorm.lengthSquared() < (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
vecnorm.set(-1f, -1f, -1f);
}
vecnorm.normalize();
supVertex.scaleAdd(getMargin(), vecnorm, supVertex);
}
return out;
}
public boolean SolveSimplex3a(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f cabc) {
// TODO: optimize
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(cabc, ab);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
tmp2.cross(cabc, ac);
if (tmp.dot(ao) < -GJK_insimplex_eps) {
order = 1;
simplex[0].set(simplex[1]);
simplex[1].set(simplex[2]);
return SolveSimplex2(ao, ab);
} else if (tmp2.dot(ao) > +GJK_insimplex_eps) {
order = 1;
simplex[1].set(simplex[2]);
return SolveSimplex2(ao, ac);
} else {
float d = cabc.dot(ao);
if (Math.abs(d) > GJK_insimplex_eps) {
if (d > 0) {
ray.set(cabc);
} else {
ray.negate(cabc);
Mkv swapTmp = new Mkv();
swapTmp.set(simplex[0]);
simplex[0].set(simplex[1]);
simplex[1].set(swapTmp);
}
return false;
} else {
return true;
}
}
}
public boolean SolveSimplex4(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f ad) {
// TODO: optimize
Vector3f crs = Stack.alloc(Vector3f.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(ab, ac);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
tmp2.cross(ac, ad);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
tmp3.cross(ad, ab);
if (tmp.dot(ao) > GJK_insimplex_eps) {
crs.set(tmp);
order = 2;
simplex[0].set(simplex[1]);
simplex[1].set(simplex[2]);
simplex[2].set(simplex[3]);
return SolveSimplex3a(ao, ab, ac, crs);
} else if (tmp2.dot(ao) > GJK_insimplex_eps) {
crs.set(tmp2);
order = 2;
simplex[2].set(simplex[3]);
return SolveSimplex3a(ao, ac, ad, crs);
} else if (tmp3.dot(ao) > GJK_insimplex_eps) {
crs.set(tmp3);
order = 2;
simplex[1].set(simplex[0]);
simplex[0].set(simplex[2]);
simplex[2].set(simplex[3]);
return SolveSimplex3a(ao, ad, ab, crs);
} else {
return (true);
}
}
public boolean SolveSimplex2(Vector3f ao, Vector3f ab) {
if (ab.dot(ao) >= 0) {
Vector3f cabo = Stack.alloc(Vector3f.class);
cabo.cross(ab, ao);
if (cabo.lengthSquared() > GJK_sqinsimplex_eps) {
ray.cross(cabo, ab);
} else {
return true;
}
} else {
order = 0;
simplex[0].set(simplex[1]);
ray.set(ao);
}
return (false);
}
protected void predictUnconstraintMotion(float timeStep) {
Transform tmpTrans = Stack.alloc(Transform.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
if (!body.isStaticOrKinematicObject()) {
if (body.isActive()) {
body.integrateVelocities(timeStep);
// damping
body.applyDamping(timeStep);
body.predictIntegratedTransform(
timeStep, body.getInterpolationWorldTransform(tmpTrans));
}
}
}
}
}
public void debugDrawObject(Transform worldTransform, CollisionShape shape, Vector3f color) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
// Draw a small simplex at the center of the object
{
Vector3f start = Stack.alloc(worldTransform.origin);
tmp.set(1f, 0f, 0f);
worldTransform.basis.transform(tmp);
tmp.add(start);
tmp2.set(1f, 0f, 0f);
getDebugDrawer().drawLine(start, tmp, tmp2);
tmp.set(0f, 1f, 0f);
worldTransform.basis.transform(tmp);
tmp.add(start);
tmp2.set(0f, 1f, 0f);
getDebugDrawer().drawLine(start, tmp, tmp2);
tmp.set(0f, 0f, 1f);
worldTransform.basis.transform(tmp);
tmp.add(start);
tmp2.set(0f, 0f, 1f);
getDebugDrawer().drawLine(start, tmp, tmp2);
}
// JAVA TODO: debugDrawObject, note that this commented code is from old version, use actual
// version when implementing
// if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
// {
// const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
// for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
// {
// btTransform childTrans = compoundShape->getChildTransform(i);
// const btCollisionShape* colShape = compoundShape->getChildShape(i);
// debugDrawObject(worldTransform*childTrans,colShape,color);
// }
//
// } else
// {
// switch (shape->getShapeType())
// {
//
// case SPHERE_SHAPE_PROXYTYPE:
// {
// const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
// btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw
// with margin
//
// debugDrawSphere(radius, worldTransform, color);
// break;
// }
// case MULTI_SPHERE_SHAPE_PROXYTYPE:
// {
// const btMultiSphereShape* multiSphereShape = static_cast<const
// btMultiSphereShape*>(shape);
//
// for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
// {
// btTransform childTransform = worldTransform;
// childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
// debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
// }
//
// break;
// }
// case CAPSULE_SHAPE_PROXYTYPE:
// {
// const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
//
// btScalar radius = capsuleShape->getRadius();
// btScalar halfHeight = capsuleShape->getHalfHeight();
//
// // Draw the ends
// {
// btTransform childTransform = worldTransform;
// childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
// debugDrawSphere(radius, childTransform, color);
// }
//
// {
// btTransform childTransform = worldTransform;
// childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
// debugDrawSphere(radius, childTransform, color);
// }
//
// // Draw some additional lines
// btVector3 start = worldTransform.getOrigin();
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() *
// btVector3(-radius,-halfHeight,0), color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// btVector3(radius,halfHeight,0),start+worldTransform.getBasis() *
// btVector3(radius,-halfHeight,0), color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() *
// btVector3(0,-halfHeight,-radius), color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// btVector3(0,halfHeight,radius),start+worldTransform.getBasis() *
// btVector3(0,-halfHeight,radius), color);
//
// break;
// }
// case CONE_SHAPE_PROXYTYPE:
// {
// const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
// btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
// btScalar height = coneShape->getHeight();//+coneShape->getMargin();
// btVector3 start = worldTransform.getOrigin();
//
// int upAxis= coneShape->getConeUpIndex();
//
//
// btVector3 offsetHeight(0,0,0);
// offsetHeight[upAxis] = height * btScalar(0.5);
// btVector3 offsetRadius(0,0,0);
// offsetRadius[(upAxis+1)%3] = radius;
// btVector3 offset2Radius(0,0,0);
// offset2Radius[(upAxis+2)%3] = radius;
//
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
//
//
//
// break;
//
// }
// case CYLINDER_SHAPE_PROXYTYPE:
// {
// const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
// int upAxis = cylinder->getUpAxis();
// btScalar radius = cylinder->getRadius();
// btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
// btVector3 start = worldTransform.getOrigin();
// btVector3 offsetHeight(0,0,0);
// offsetHeight[upAxis] = halfHeight;
// btVector3 offsetRadius(0,0,0);
// offsetRadius[(upAxis+1)%3] = radius;
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// (offsetHeight+offsetRadius),start+worldTransform.getBasis() *
// (-offsetHeight+offsetRadius),color);
// getDebugDrawer()->drawLine(start+worldTransform.getBasis() *
// (offsetHeight-offsetRadius),start+worldTransform.getBasis() *
// (-offsetHeight-offsetRadius),color);
// break;
// }
// default:
// {
//
// if (shape->isConcave())
// {
// btConcaveShape* concaveMesh = (btConcaveShape*) shape;
//
// //todo pass camera, for some culling
// btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
// btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
//
// DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
// concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
//
// }
//
// if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
// {
// btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
// //todo: pass camera for some culling
// btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
// btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
// //DebugDrawcallback drawCallback;
// DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
//
// convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
// }
//
//
// /// for polyhedral shapes
// if (shape->isPolyhedral())
// {
// btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
//
// int i;
// for (i=0;i<polyshape->getNumEdges();i++)
// {
// btPoint3 a,b;
// polyshape->getEdge(i,a,b);
// btVector3 wa = worldTransform * a;
// btVector3 wb = worldTransform * b;
// getDebugDrawer()->drawLine(wa,wb,color);
//
// }
//
//
// }
// }
// }
// }
}
public boolean SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac) {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(ab, ac);
return (SolveSimplex3a(ao, ab, ac, tmp));
}
@Override
public void debugDrawWorld() {
if (getDebugDrawer() != null
&& (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) {
int numManifolds = getDispatcher().getNumManifolds();
Vector3f color = Stack.alloc(Vector3f.class);
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.alloc(Transform.class);
Vector3f minAabb = Stack.alloc(Vector3f.class);
Vector3f maxAabb = Stack.alloc(Vector3f.class);
Vector3f colorvec = Stack.alloc(Vector3f.class);
// 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.alloc(Vector3f.class);
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.alloc(Vector3f.class);
Vector3f wheelPosWS = Stack.alloc(Vector3f.class);
Vector3f axle = Stack.alloc(Vector3f.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
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);
}
}
}
}
public boolean SearchOrigin() {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.set(1f, 0f, 0f);
return SearchOrigin(tmp);
}
public boolean EncloseOrigin() {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
switch (order) {
// Point
case 0:
break;
// Line
case 1:
{
Vector3f ab = Stack.alloc(Vector3f.class);
ab.sub(simplex[1].w, simplex[0].w);
Vector3f[] b =
new Vector3f[] {
Stack.alloc(Vector3f.class),
Stack.alloc(Vector3f.class),
Stack.alloc(Vector3f.class)
};
b[0].set(1f, 0f, 0f);
b[1].set(0f, 1f, 0f);
b[2].set(0f, 0f, 1f);
b[0].cross(ab, b[0]);
b[1].cross(ab, b[1]);
b[2].cross(ab, b[2]);
float m[] =
new float[] {b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared()};
Quat4f tmpQuat = Stack.alloc(Quat4f.class);
tmp.normalize(ab);
QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f);
Matrix3f r = Stack.alloc(Matrix3f.class);
MatrixUtil.setRotation(r, tmpQuat);
Vector3f w = Stack.alloc(Vector3f.class);
w.set(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]);
tmp.normalize(w);
Support(tmp, simplex[4]);
r.transform(w);
tmp.normalize(w);
Support(tmp, simplex[2]);
r.transform(w);
tmp.normalize(w);
Support(tmp, simplex[3]);
r.transform(w);
order = 4;
return (true);
}
// Triangle
case 2:
{
tmp1.sub(simplex[1].w, simplex[0].w);
tmp2.sub(simplex[2].w, simplex[0].w);
Vector3f n = Stack.alloc(Vector3f.class);
n.cross(tmp1, tmp2);
n.normalize();
Support(n, simplex[3]);
tmp.negate(n);
Support(tmp, simplex[4]);
order = 4;
return (true);
}
// Tetrahedron
case 3:
return (true);
// Hexahedron
case 4:
return (true);
}
return (false);
}
public float EvaluatePD(float accuracy) {
pushStack();
try {
Vector3f tmp = Stack.alloc(Vector3f.class);
// btBlock* sablock = sa->beginBlock();
Face bestface = null;
int markid = 1;
depth = -cstInf;
normal.set(0f, 0f, 0f);
root = null;
nfaces = 0;
iterations = 0;
failed = false;
/* Prepare hull */
if (gjk.EncloseOrigin()) {
// const U* pfidx = 0;
int[][] pfidx_ptr = null;
int pfidx_index = 0;
int nfidx = 0;
// const U* peidx = 0;
int[][] peidx_ptr = null;
int peidx_index = 0;
int neidx = 0;
Mkv[] basemkv = new Mkv[5];
Face[] basefaces = new Face[6];
switch (gjk.order) {
// Tetrahedron
case 3:
{
// pfidx=(const U*)fidx;
pfidx_ptr = tetrahedron_fidx;
pfidx_index = 0;
nfidx = 4;
// peidx=(const U*)eidx;
peidx_ptr = tetrahedron_eidx;
peidx_index = 0;
neidx = 6;
}
break;
// Hexahedron
case 4:
{
// pfidx=(const U*)fidx;
pfidx_ptr = hexahedron_fidx;
pfidx_index = 0;
nfidx = 6;
// peidx=(const U*)eidx;
peidx_ptr = hexahedron_eidx;
peidx_index = 0;
neidx = 9;
}
break;
}
int i;
for (i = 0; i <= gjk.order; ++i) {
basemkv[i] = new Mkv();
basemkv[i].set(gjk.simplex[i]);
}
for (i = 0; i < nfidx; ++i, pfidx_index++) {
basefaces[i] =
NewFace(
basemkv[pfidx_ptr[pfidx_index][0]],
basemkv[pfidx_ptr[pfidx_index][1]],
basemkv[pfidx_ptr[pfidx_index][2]]);
}
for (i = 0; i < neidx; ++i, peidx_index++) {
Link(
basefaces[peidx_ptr[peidx_index][0]],
peidx_ptr[peidx_index][1],
basefaces[peidx_ptr[peidx_index][2]],
peidx_ptr[peidx_index][3]);
}
}
if (0 == nfaces) {
// sa->endBlock(sablock);
return (depth);
}
/* Expand hull */
for (; iterations < EPA_maxiterations; ++iterations) {
Face bf = FindBest();
if (bf != null) {
tmp.negate(bf.n);
Mkv w = Support(tmp);
float d = bf.n.dot(w.w) + bf.d;
bestface = bf;
if (d < -accuracy) {
Face[] cf = new Face[] {null};
Face[] ff = new Face[] {null};
int nf = 0;
Detach(bf);
bf.mark = ++markid;
for (int i = 0; i < 3; ++i) {
nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff);
}
if (nf <= 2) {
break;
}
Link(cf[0], 1, ff[0], 2);
} else {
break;
}
} else {
break;
}
}
/* Extract contact */
if (bestface != null) {
Vector3f b = GetCoordinates(bestface, Stack.alloc(Vector3f.class));
normal.set(bestface.n);
depth = Math.max(0, bestface.d);
for (int i = 0; i < 2; ++i) {
float s = i != 0 ? -1f : 1f;
for (int j = 0; j < 3; ++j) {
tmp.scale(s, bestface.v[j].r);
gjk.LocalSupport(tmp, i, features[i][j]);
}
}
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
tmp1.scale(b.x, features[0][0]);
tmp2.scale(b.y, features[0][1]);
tmp3.scale(b.z, features[0][2]);
VectorUtil.add(nearest[0], tmp1, tmp2, tmp3);
tmp1.scale(b.x, features[1][0]);
tmp2.scale(b.y, features[1][1]);
tmp3.scale(b.z, features[1][2]);
VectorUtil.add(nearest[1], tmp1, tmp2, tmp3);
} else {
failed = true;
}
// sa->endBlock(sablock);
return (depth);
} finally {
popStack();
}
}
@Override
public void solveConstraint(float timeStep) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmpVec = Stack.alloc(Vector3f.class);
Transform tmpTrans = Stack.alloc(Transform.class);
Vector3f pivotAInW = Stack.alloc(rbAFrame.origin);
rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
Vector3f pivotBInW = Stack.alloc(rbBFrame.origin);
rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
float tau = 0.3f;
// linear part
if (!angularOnly) {
Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
Vector3f vel1 = rbA.getVelocityInLocalPoint(rel_pos1, Stack.alloc(Vector3f.class));
Vector3f vel2 = rbB.getVelocityInLocalPoint(rel_pos2, Stack.alloc(Vector3f.class));
Vector3f vel = Stack.alloc(Vector3f.class);
vel.sub(vel1, vel2);
for (int i = 0; i < 3; i++) {
Vector3f normal = jac[i].linearJointAxis;
float jacDiagABInv = 1f / jac[i].getDiagonal();
float rel_vel;
rel_vel = normal.dot(vel);
// positional error (zeroth order error)
tmp.sub(pivotAInW, pivotBInW);
float depth = -(tmp).dot(normal); // this is the error projected on the normal
float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
appliedImpulse += impulse;
Vector3f impulse_vector = Stack.alloc(Vector3f.class);
impulse_vector.scale(impulse, normal);
tmp.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
rbA.applyImpulse(impulse_vector, tmp);
tmp.negate(impulse_vector);
tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
rbB.applyImpulse(tmp, tmp2);
}
}
{
// solve angular part
Vector3f angVelA = getRigidBodyA().getAngularVelocity(Stack.alloc(Vector3f.class));
Vector3f angVelB = getRigidBodyB().getAngularVelocity(Stack.alloc(Vector3f.class));
// solve swing limit
if (solveSwingLimit) {
tmp.sub(angVelB, angVelA);
float amplitude =
((tmp).dot(swingAxis) * relaxationFactor * relaxationFactor
+ swingCorrection * (1f / timeStep) * biasFactor);
float impulseMag = amplitude * kSwing;
// Clamp the accumulated impulse
float temp = accSwingLimitImpulse;
accSwingLimitImpulse = Math.max(accSwingLimitImpulse + impulseMag, 0.0f);
impulseMag = accSwingLimitImpulse - temp;
Vector3f impulse = Stack.alloc(Vector3f.class);
impulse.scale(impulseMag, swingAxis);
rbA.applyTorqueImpulse(impulse);
tmp.negate(impulse);
rbB.applyTorqueImpulse(tmp);
}
// solve twist limit
if (solveTwistLimit) {
tmp.sub(angVelB, angVelA);
float amplitude =
((tmp).dot(twistAxis) * relaxationFactor * relaxationFactor
+ twistCorrection * (1f / timeStep) * biasFactor);
float impulseMag = amplitude * kTwist;
// Clamp the accumulated impulse
float temp = accTwistLimitImpulse;
accTwistLimitImpulse = Math.max(accTwistLimitImpulse + impulseMag, 0.0f);
impulseMag = accTwistLimitImpulse - temp;
Vector3f impulse = Stack.alloc(Vector3f.class);
impulse.scale(impulseMag, twistAxis);
rbA.applyTorqueImpulse(impulse);
tmp.negate(impulse);
rbB.applyTorqueImpulse(tmp);
}
}
}
@Override
public void buildJacobian() {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Transform tmpTrans = Stack.alloc(Transform.class);
appliedImpulse = 0f;
// set bias, sign, clear accumulator
swingCorrection = 0f;
twistLimitSign = 0f;
solveTwistLimit = false;
solveSwingLimit = false;
accTwistLimitImpulse = 0f;
accSwingLimitImpulse = 0f;
if (!angularOnly) {
Vector3f pivotAInW = Stack.alloc(rbAFrame.origin);
rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
Vector3f pivotBInW = Stack.alloc(rbBFrame.origin);
rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
Vector3f relPos = Stack.alloc(Vector3f.class);
relPos.sub(pivotBInW, pivotAInW);
// TODO: stack
Vector3f[] normal /*[3]*/ =
new Vector3f[] {
Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class)
};
if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
normal[0].normalize(relPos);
} else {
normal[0].set(1f, 0f, 0f);
}
TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) {
Matrix3f mat1 = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
mat1.transpose();
Matrix3f mat2 = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
mat2.transpose();
tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmp));
tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmp));
jac[i].init(
mat1,
mat2,
tmp1,
tmp2,
normal[i],
rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
rbA.getInvMass(),
rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
rbB.getInvMass());
}
}
Vector3f b1Axis1 = Stack.alloc(Vector3f.class),
b1Axis2 = Stack.alloc(Vector3f.class),
b1Axis3 = Stack.alloc(Vector3f.class);
Vector3f b2Axis1 = Stack.alloc(Vector3f.class), b2Axis2 = Stack.alloc(Vector3f.class);
rbAFrame.basis.getColumn(0, b1Axis1);
getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis1);
rbBFrame.basis.getColumn(0, b2Axis1);
getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis1);
float swing1 = 0f, swing2 = 0f;
float swx = 0f, swy = 0f;
float thresh = 10f;
float fact;
// Get Frame into world space
if (swingSpan1 >= 0.05f) {
rbAFrame.basis.getColumn(1, b1Axis2);
getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis2);
// swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = ScalarUtil.atan2Fast(swy, swx);
fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + 1f);
swing1 *= fact;
}
if (swingSpan2 >= 0.05f) {
rbAFrame.basis.getColumn(2, b1Axis3);
getRigidBodyA().getCenterOfMassTransform(tmpTrans).basis.transform(b1Axis3);
// swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = ScalarUtil.atan2Fast(swy, swx);
fact = (swy * swy + swx * swx) * thresh * thresh;
fact = fact / (fact + 1f);
swing2 *= fact;
}
float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
float EllipseAngle =
Math.abs(swing1 * swing1) * RMaxAngle1Sq + Math.abs(swing2 * swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f) {
swingCorrection = EllipseAngle - 1.0f;
solveSwingLimit = true;
// Calculate necessary axis & factors
tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
tmp.add(tmp1, tmp2);
swingAxis.cross(b2Axis1, tmp);
swingAxis.normalize();
float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
swingAxis.scale(swingAxisSign);
kSwing =
1f
/ (getRigidBodyA().computeAngularImpulseDenominator(swingAxis)
+ getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
}
// Twist limits
if (twistSpan >= 0f) {
// Vector3f b2Axis2 = Stack.alloc(Vector3f.class);
rbBFrame.basis.getColumn(1, b2Axis2);
getRigidBodyB().getCenterOfMassTransform(tmpTrans).basis.transform(b2Axis2);
Quat4f rotationArc =
QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1, Stack.alloc(Quat4f.class));
Vector3f TwistRef =
QuaternionUtil.quatRotate(rotationArc, b2Axis2, Stack.alloc(Vector3f.class));
float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
if (twist <= -twistSpan * lockedFreeFactor) {
twistCorrection = -(twist + twistSpan);
solveTwistLimit = true;
twistAxis.add(b2Axis1, b1Axis1);
twistAxis.scale(0.5f);
twistAxis.normalize();
twistAxis.scale(-1.0f);
kTwist =
1f
/ (getRigidBodyA().computeAngularImpulseDenominator(twistAxis)
+ getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
} else if (twist > twistSpan * lockedFreeFactor) {
twistCorrection = (twist - twistSpan);
solveTwistLimit = true;
twistAxis.add(b2Axis1, b1Axis1);
twistAxis.scale(0.5f);
twistAxis.normalize();
kTwist =
1f
/ (getRigidBodyA().computeAngularImpulseDenominator(twistAxis)
+ getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
}
}
}