Examples with StaticAlloc com.bulletphysics.util.StaticAlloc used on opensource projects

Example 6 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class TransformUtil method integrateTransform.

@StaticAlloc
public static void integrateTransform(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Transform predictedTransform) {
    predictedTransform.origin.scaleAdd(timeStep, linvel, curTrans.origin);
    //	//#define QUATERNION_DERIVATIVE
    //	#ifdef QUATERNION_DERIVATIVE
    //		btQuaternion predictedOrn = curTrans.getRotation();
    //		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
    //		predictedOrn.normalize();
    //	#else
    // Exponential map
    // google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
    Stack stack = Stack.enter();
    Vector3f axis = stack.allocVector3f();
    float fAngle = angvel.length();
    // limit the angular motion
    if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) {
        fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
    }
    if (fAngle < 0.001f) {
        // use Taylor's expansions of sync function
        axis.scale(0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle, angvel);
    } else {
        // sync(fAngle) = sin(c*fAngle)/t
        axis.scale((float) Math.sin(0.5f * fAngle * timeStep) / fAngle, angvel);
    }
    Quat4f dorn = stack.allocQuat4f();
    dorn.set(axis.x, axis.y, axis.z, (float) Math.cos(fAngle * timeStep * 0.5f));
    Quat4f orn0 = curTrans.getRotation(stack.allocQuat4f());
    Quat4f predictedOrn = stack.allocQuat4f();
    predictedOrn.mul(dorn, orn0);
    predictedOrn.normalize();
    //  #endif
    predictedTransform.setRotation(predictedOrn);
    stack.leave();
}

Example 7 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class GjkPairDetector method getClosestPoints.

@StaticAlloc
public void getClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw, boolean swapResults) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    float distance = 0f;
    Vector3f normalInB = stack.allocVector3f();
    normalInB.set(0f, 0f, 0f);
    Vector3f pointOnA = stack.allocVector3f(), pointOnB = stack.allocVector3f();
    Transform localTransA = stack.alloc(input.transformA);
    Transform localTransB = stack.alloc(input.transformB);
    Vector3f positionOffset = stack.allocVector3f();
    positionOffset.add(localTransA.origin, localTransB.origin);
    positionOffset.scale(0.5f);
    localTransA.origin.sub(positionOffset);
    localTransB.origin.sub(positionOffset);
    float marginA = minkowskiA.getMargin();
    float marginB = minkowskiB.getMargin();
    BulletStats.gNumGjkChecks++;
    // for CCD we don't use margins
    if (ignoreMargin) {
        marginA = 0f;
        marginB = 0f;
    }
    curIter = 0;
    // this is to catch invalid input, perhaps check for #NaN?
    int gGjkMaxIter = 1000;
    cachedSeparatingAxis.set(0f, 1f, 0f);
    boolean isValid = false;
    boolean checkSimplex = false;
    boolean checkPenetration = true;
    degenerateSimplex = 0;
    lastUsedMethod = -1;
    {
        float squaredDistance = BulletGlobals.SIMD_INFINITY;
        float delta = 0f;
        float margin = marginA + marginB;
        simplexSolver.reset();
        Vector3f seperatingAxisInA = stack.allocVector3f();
        Vector3f seperatingAxisInB = stack.allocVector3f();
        Vector3f pInA = stack.allocVector3f();
        Vector3f qInB = stack.allocVector3f();
        Vector3f pWorld = stack.allocVector3f();
        Vector3f qWorld = stack.allocVector3f();
        Vector3f w = stack.allocVector3f();
        Vector3f tmpPointOnA = stack.allocVector3f(), tmpPointOnB = stack.allocVector3f();
        Vector3f tmpNormalInB = stack.allocVector3f();
        for (; ; ) //while (true)
        {
            seperatingAxisInA.negate(cachedSeparatingAxis);
            MatrixUtil.transposeTransform(seperatingAxisInA, seperatingAxisInA, input.transformA.basis);
            seperatingAxisInB.set(cachedSeparatingAxis);
            MatrixUtil.transposeTransform(seperatingAxisInB, seperatingAxisInB, input.transformB.basis);
            minkowskiA.localGetSupportingVertexWithoutMargin(seperatingAxisInA, pInA);
            minkowskiB.localGetSupportingVertexWithoutMargin(seperatingAxisInB, qInB);
            pWorld.set(pInA);
            localTransA.transform(pWorld);
            qWorld.set(qInB);
            localTransB.transform(qWorld);
            w.sub(pWorld, qWorld);
            delta = cachedSeparatingAxis.dot(w);
            // potential exit, they don't overlap
            if ((delta > 0f) && (delta * delta > squaredDistance * input.maximumDistanceSquared)) {
                checkPenetration = false;
                break;
            }
            // exit 0: the new point is already in the simplex, or we didn't come any closer
            if (simplexSolver.inSimplex(w)) {
                degenerateSimplex = 1;
                checkSimplex = true;
                break;
            }
            // are we getting any closer ?
            float f0 = squaredDistance - delta;
            float f1 = squaredDistance * REL_ERROR2;
            if (f0 <= f1) {
                if (f0 <= 0f) {
                    degenerateSimplex = 2;
                }
                checkSimplex = true;
                break;
            }
            // add current vertex to simplex
            simplexSolver.addVertex(w, pWorld, qWorld);
            // calculate the closest point to the origin (update vector v)
            if (!simplexSolver.closest(cachedSeparatingAxis)) {
                degenerateSimplex = 3;
                checkSimplex = true;
                break;
            }
            if (cachedSeparatingAxis.lengthSquared() < REL_ERROR2) {
                degenerateSimplex = 6;
                checkSimplex = true;
                break;
            }
            float previousSquaredDistance = squaredDistance;
            squaredDistance = cachedSeparatingAxis.lengthSquared();
            // are we getting any closer ?
            if (previousSquaredDistance - squaredDistance <= BulletGlobals.FLT_EPSILON * previousSquaredDistance) {
                simplexSolver.backup_closest(cachedSeparatingAxis);
                checkSimplex = true;
                break;
            }
            // degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject   
            if (curIter++ > gGjkMaxIter) {
                //#if defined(DEBUG) || defined (_DEBUG)   
                if (BulletGlobals.DEBUG) {
                    System.err.println("btGjkPairDetector maxIter exceeded:" + curIter);
                /*System.err.printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
								cachedSeparatingAxis.x,
								cachedSeparatingAxis.y,
								cachedSeparatingAxis.z,
								squaredDistance,
								minkowskiA.getShapeType(),
								minkowskiB.getShapeType());*/
                }
                //#endif   
                break;
            }
            boolean check = (!simplexSolver.fullSimplex());
            if (!check) {
                // do we need this backup_closest here ?
                simplexSolver.backup_closest(cachedSeparatingAxis);
                break;
            }
        }
        if (checkSimplex) {
            simplexSolver.compute_points(pointOnA, pointOnB);
            normalInB.sub(pointOnA, pointOnB);
            float lenSqr = cachedSeparatingAxis.lengthSquared();
            // valid normal
            if (lenSqr < 0.0001f) {
                degenerateSimplex = 5;
            }
            if (lenSqr > BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
                float rlen = 1f / (float) Math.sqrt(lenSqr);
                // normalize
                normalInB.scale(rlen);
                float s = (float) Math.sqrt(squaredDistance);
                assert (s > 0f);
                tmp.scale((marginA / s), cachedSeparatingAxis);
                pointOnA.sub(tmp);
                tmp.scale((marginB / s), cachedSeparatingAxis);
                pointOnB.add(tmp);
                distance = ((1f / rlen) - margin);
                isValid = true;
                lastUsedMethod = 1;
            } else {
                lastUsedMethod = 2;
            }
        }
        boolean catchDegeneratePenetrationCase = (catchDegeneracies != 0 && penetrationDepthSolver != null && degenerateSimplex != 0 && ((distance + margin) < 0.01f));
        //if (checkPenetration && !isValid)
        if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) {
            // if there is no way to handle penetrations, bail out
            if (penetrationDepthSolver != null) {
                // Penetration depth case.
                BulletStats.gNumDeepPenetrationChecks++;
                boolean isValid2 = penetrationDepthSolver.calcPenDepth(simplexSolver, minkowskiA, minkowskiB, localTransA, localTransB, cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, debugDraw);
                if (isValid2) {
                    tmpNormalInB.sub(tmpPointOnB, tmpPointOnA);
                    float lenSqr = tmpNormalInB.lengthSquared();
                    if (lenSqr > (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
                        tmpNormalInB.scale(1f / (float) Math.sqrt(lenSqr));
                        tmp.sub(tmpPointOnA, tmpPointOnB);
                        float distance2 = -tmp.length();
                        // only replace valid penetrations when the result is deeper (check)
                        if (!isValid || (distance2 < distance)) {
                            distance = distance2;
                            pointOnA.set(tmpPointOnA);
                            pointOnB.set(tmpPointOnB);
                            normalInB.set(tmpNormalInB);
                            isValid = true;
                            lastUsedMethod = 3;
                        } else {
                        }
                    } else {
                        //isValid = false;
                        lastUsedMethod = 4;
                    }
                } else {
                    lastUsedMethod = 5;
                }
            }
        }
    }
    if (isValid) {
        //#ifdef __SPU__
        //		//spu_printf("distance\n");
        //#endif //__CELLOS_LV2__
        tmp.add(pointOnB, positionOffset);
        output.addContactPoint(normalInB, tmp, distance);
    //printf("gjk add:%f",distance);
    }
    stack.leave();
}

Example 8 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class VoronoiSimplexSolver method closestPtPointTetrahedron.

@StaticAlloc
public boolean closestPtPointTetrahedron(Vector3f p, Vector3f a, Vector3f b, Vector3f c, Vector3f d, SubSimplexClosestResult finalResult) {
    SubSimplexClosestResult tempResult = subsimplexResultsPool.get();
    tempResult.reset();
    Stack stack = Stack.enter();
    int sp = stack.getSp();
    try {
        Vector3f tmp = stack.allocVector3f();
        Vector3f q = stack.allocVector3f();
        // Start out assuming point inside all halfspaces, so closest to itself
        finalResult.closestPointOnSimplex.set(p);
        finalResult.usedVertices.reset();
        finalResult.usedVertices.usedVertexA = true;
        finalResult.usedVertices.usedVertexB = true;
        finalResult.usedVertices.usedVertexC = true;
        finalResult.usedVertices.usedVertexD = true;
        int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
        int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
        int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
        int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
        if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) {
            finalResult.degenerate = true;
            return false;
        }
        if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) {
            return false;
        }
        float bestSqDist = Float.MAX_VALUE;
        // If point outside face abc then compute closest point on abc
        if (pointOutsideABC != 0) {
            closestPtPointTriangle(p, a, b, c, tempResult);
            q.set(tempResult.closestPointOnSimplex);
            tmp.sub(q, p);
            float sqDist = tmp.dot(tmp);
            // Update best closest point if (squared) distance is less than current best
            if (sqDist < bestSqDist) {
                bestSqDist = sqDist;
                finalResult.closestPointOnSimplex.set(q);
                //convert result bitmask!
                finalResult.usedVertices.reset();
                finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
                finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexB;
                finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC;
                finalResult.setBarycentricCoordinates(tempResult.barycentricCoords[VERTA], tempResult.barycentricCoords[VERTB], tempResult.barycentricCoords[VERTC], 0);
            }
        }
        // Repeat test for face acd
        if (pointOutsideACD != 0) {
            closestPtPointTriangle(p, a, c, d, tempResult);
            q.set(tempResult.closestPointOnSimplex);
            //convert result bitmask!
            tmp.sub(q, p);
            float sqDist = tmp.dot(tmp);
            if (sqDist < bestSqDist) {
                bestSqDist = sqDist;
                finalResult.closestPointOnSimplex.set(q);
                finalResult.usedVertices.reset();
                finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
                finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexB;
                finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexC;
                finalResult.setBarycentricCoordinates(tempResult.barycentricCoords[VERTA], 0, tempResult.barycentricCoords[VERTB], tempResult.barycentricCoords[VERTC]);
            }
        }
        if (pointOutsideADB != 0) {
            closestPtPointTriangle(p, a, d, b, tempResult);
            q.set(tempResult.closestPointOnSimplex);
            //convert result bitmask!
            tmp.sub(q, p);
            float sqDist = tmp.dot(tmp);
            if (sqDist < bestSqDist) {
                bestSqDist = sqDist;
                finalResult.closestPointOnSimplex.set(q);
                finalResult.usedVertices.reset();
                finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
                finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexC;
                finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB;
                finalResult.setBarycentricCoordinates(tempResult.barycentricCoords[VERTA], tempResult.barycentricCoords[VERTC], 0, tempResult.barycentricCoords[VERTB]);
            }
        }
        if (pointOutsideBDC != 0) {
            closestPtPointTriangle(p, b, d, c, tempResult);
            q.set(tempResult.closestPointOnSimplex);
            //convert result bitmask!
            tmp.sub(q, p);
            float sqDist = tmp.dot(tmp);
            if (sqDist < bestSqDist) {
                bestSqDist = sqDist;
                finalResult.closestPointOnSimplex.set(q);
                finalResult.usedVertices.reset();
                //
                finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexA;
                finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC;
                finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB;
                finalResult.setBarycentricCoordinates(0, tempResult.barycentricCoords[VERTA], tempResult.barycentricCoords[VERTC], tempResult.barycentricCoords[VERTB]);
            }
        }
        if (finalResult.usedVertices.usedVertexA && finalResult.usedVertices.usedVertexB && finalResult.usedVertices.usedVertexC && finalResult.usedVertices.usedVertexD) {
            return true;
        }
        return true;
    } finally {
        subsimplexResultsPool.release(tempResult);
        stack.leave(sp);
    }
}

Example 9 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class RigidBody method applyTorqueImpulse.

@StaticAlloc
public void applyTorqueImpulse(Vector3f torque) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.alloc(torque);
    invInertiaTensorWorld.transform(tmp);
    angularVelocity.add(tmp);
    stack.leave();
}

Example 10 with StaticAlloc

use of com.bulletphysics.util.StaticAlloc in project bdx by GoranM.

the class RotationalLimitMotor method solveAngularLimits.

/**
	 * Apply the correction impulses for two bodies.
	 */
@StaticAlloc
public float solveAngularLimits(float timeStep, Vector3f axis, float jacDiagABInv, RigidBody body0, RigidBody body1) {
    if (needApplyTorques() == false) {
        return 0.0f;
    }
    Stack stack = Stack.enter();
    float target_velocity = this.targetVelocity;
    float maxMotorForce = this.maxMotorForce;
    // current error correction
    if (currentLimit != 0) {
        target_velocity = -ERP * currentLimitError / (timeStep);
        maxMotorForce = maxLimitForce;
    }
    maxMotorForce *= timeStep;
    // current velocity difference
    Vector3f vel_diff = body0.getAngularVelocity(stack.allocVector3f());
    if (body1 != null) {
        vel_diff.sub(body1.getAngularVelocity(stack.allocVector3f()));
    }
    float rel_vel = axis.dot(vel_diff);
    // correction velocity
    float motor_relvel = limitSoftness * (target_velocity - damping * rel_vel);
    if (motor_relvel < BulletGlobals.FLT_EPSILON && motor_relvel > -BulletGlobals.FLT_EPSILON) {
        // no need for applying force
        return 0.0f;
    }
    // correction impulse
    float unclippedMotorImpulse = (1 + bounce) * motor_relvel * jacDiagABInv;
    // clip correction impulse
    float clippedMotorImpulse;
    // todo: should clip against accumulated impulse
    if (unclippedMotorImpulse > 0.0f) {
        clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
    } else {
        clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
    }
    // sort with accumulated impulses
    float lo = -1e30f;
    float hi = 1e30f;
    float oldaccumImpulse = accumulatedImpulse;
    float sum = oldaccumImpulse + clippedMotorImpulse;
    accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
    clippedMotorImpulse = accumulatedImpulse - oldaccumImpulse;
    Vector3f motorImp = stack.allocVector3f();
    motorImp.scale(clippedMotorImpulse, axis);
    body0.applyTorqueImpulse(motorImp);
    if (body1 != null) {
        motorImp.negate();
        body1.applyTorqueImpulse(motorImp);
    }
    stack.leave();
    return clippedMotorImpulse;
}