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

Example 1 with StaticAlloc

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

the class VoronoiSimplexSolver method closestPtPointTriangle.

@StaticAlloc
public boolean closestPtPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c, SubSimplexClosestResult result) {
    result.usedVertices.reset();
    Stack stack = Stack.enter();
    // Check if P in vertex region outside A
    Vector3f ab = stack.allocVector3f();
    ab.sub(b, a);
    Vector3f ac = stack.allocVector3f();
    ac.sub(c, a);
    Vector3f ap = stack.allocVector3f();
    ap.sub(p, a);
    float d1 = ab.dot(ap);
    float d2 = ac.dot(ap);
    if (d1 <= 0f && d2 <= 0f) {
        result.closestPointOnSimplex.set(a);
        result.usedVertices.usedVertexA = true;
        result.setBarycentricCoordinates(1f, 0f, 0f, 0f);
        stack.leave();
        // a; // barycentric coordinates (1,0,0)
        return true;
    }
    // Check if P in vertex region outside B
    Vector3f bp = stack.allocVector3f();
    bp.sub(p, b);
    float d3 = ab.dot(bp);
    float d4 = ac.dot(bp);
    if (d3 >= 0f && d4 <= d3) {
        result.closestPointOnSimplex.set(b);
        result.usedVertices.usedVertexB = true;
        result.setBarycentricCoordinates(0, 1f, 0f, 0f);
        stack.leave();
        // b; // barycentric coordinates (0,1,0)
        return true;
    }
    // Check if P in edge region of AB, if so return projection of P onto AB
    float vc = d1 * d4 - d3 * d2;
    if (vc <= 0f && d1 >= 0f && d3 <= 0f) {
        float v = d1 / (d1 - d3);
        result.closestPointOnSimplex.scaleAdd(v, ab, a);
        result.usedVertices.usedVertexA = true;
        result.usedVertices.usedVertexB = true;
        result.setBarycentricCoordinates(1f - v, v, 0f, 0f);
        stack.leave();
        return true;
    //return a + v * ab; // barycentric coordinates (1-v,v,0)
    }
    // Check if P in vertex region outside C
    Vector3f cp = stack.allocVector3f();
    cp.sub(p, c);
    float d5 = ab.dot(cp);
    float d6 = ac.dot(cp);
    if (d6 >= 0f && d5 <= d6) {
        result.closestPointOnSimplex.set(c);
        result.usedVertices.usedVertexC = true;
        result.setBarycentricCoordinates(0f, 0f, 1f, 0f);
        stack.leave();
        //c; // barycentric coordinates (0,0,1)
        return true;
    }
    // Check if P in edge region of AC, if so return projection of P onto AC
    float vb = d5 * d2 - d1 * d6;
    if (vb <= 0f && d2 >= 0f && d6 <= 0f) {
        float w = d2 / (d2 - d6);
        result.closestPointOnSimplex.scaleAdd(w, ac, a);
        result.usedVertices.usedVertexA = true;
        result.usedVertices.usedVertexC = true;
        result.setBarycentricCoordinates(1f - w, 0f, w, 0f);
        stack.leave();
        return true;
    //return a + w * ac; // barycentric coordinates (1-w,0,w)
    }
    // Check if P in edge region of BC, if so return projection of P onto BC
    float va = d3 * d6 - d5 * d4;
    if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) {
        float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
        Vector3f tmp = stack.allocVector3f();
        tmp.sub(c, b);
        result.closestPointOnSimplex.scaleAdd(w, tmp, b);
        result.usedVertices.usedVertexB = true;
        result.usedVertices.usedVertexC = true;
        result.setBarycentricCoordinates(0, 1f - w, w, 0f);
        stack.leave();
        return true;
    // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
    }
    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    float denom = 1f / (va + vb + vc);
    float v = vb * denom;
    float w = vc * denom;
    Vector3f tmp1 = stack.allocVector3f();
    Vector3f tmp2 = stack.allocVector3f();
    tmp1.scale(v, ab);
    tmp2.scale(w, ac);
    VectorUtil.add(result.closestPointOnSimplex, a, tmp1, tmp2);
    result.usedVertices.usedVertexA = true;
    result.usedVertices.usedVertexB = true;
    result.usedVertices.usedVertexC = true;
    result.setBarycentricCoordinates(1f - v - w, v, w, 0f);
    stack.leave();
    return true;
//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
}

Example 2 with StaticAlloc

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

the class VoronoiSimplexSolver method pointOutsideOfPlane.

/// Test if point p and d lie on opposite sides of plane through abc
@StaticAlloc
public static int pointOutsideOfPlane(Vector3f p, Vector3f a, Vector3f b, Vector3f c, Vector3f d) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    Vector3f normal = stack.allocVector3f();
    normal.sub(b, a);
    tmp.sub(c, a);
    normal.cross(normal, tmp);
    tmp.sub(p, a);
    // [AP AB AC]
    float signp = tmp.dot(normal);
    tmp.sub(d, a);
    // [AD AB AC]
    float signd = tmp.dot(normal);
    //#ifdef CATCH_DEGENERATE_TETRAHEDRON
    //	#ifdef BT_USE_DOUBLE_PRECISION
    //	if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
    //		{
    //			return -1;
    //		}
    //	#else
    stack.leave();
    if (signd * signd < ((1e-4f) * (1e-4f))) {
        //		printf("affine dependent/degenerate\n");//
        return -1;
    }
    // Points on opposite sides if expression signs are opposite
    return (signp * signd < 0f) ? 1 : 0;
}

Example 3 with StaticAlloc

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

the class VoronoiSimplexSolver method updateClosestVectorAndPoints.

@StaticAlloc
public boolean updateClosestVectorAndPoints() {
    if (needsUpdate) {
        cachedBC.reset();
        Stack stack = Stack.enter();
        needsUpdate = false;
        switch(numVertices()) {
            case 0:
                cachedValidClosest = false;
                break;
            case 1:
                {
                    cachedP1.set(simplexPointsP[0]);
                    cachedP2.set(simplexPointsQ[0]);
                    //== m_simplexVectorW[0]
                    cachedV.sub(cachedP1, cachedP2);
                    cachedBC.reset();
                    cachedBC.setBarycentricCoordinates(1f, 0f, 0f, 0f);
                    cachedValidClosest = cachedBC.isValid();
                    break;
                }
            case 2:
                {
                    Vector3f tmp = stack.allocVector3f();
                    //closest point origin from line segment
                    Vector3f from = simplexVectorW[0];
                    Vector3f to = simplexVectorW[1];
                    Vector3f nearest = stack.allocVector3f();
                    Vector3f p = stack.allocVector3f();
                    p.set(0f, 0f, 0f);
                    Vector3f diff = stack.allocVector3f();
                    diff.sub(p, from);
                    Vector3f v = stack.allocVector3f();
                    v.sub(to, from);
                    float t = v.dot(diff);
                    if (t > 0) {
                        float dotVV = v.dot(v);
                        if (t < dotVV) {
                            t /= dotVV;
                            tmp.scale(t, v);
                            diff.sub(tmp);
                            cachedBC.usedVertices.usedVertexA = true;
                            cachedBC.usedVertices.usedVertexB = true;
                        } else {
                            t = 1;
                            diff.sub(v);
                            // reduce to 1 point
                            cachedBC.usedVertices.usedVertexB = true;
                        }
                    } else {
                        t = 0;
                        //reduce to 1 point
                        cachedBC.usedVertices.usedVertexA = true;
                    }
                    cachedBC.setBarycentricCoordinates(1f - t, t, 0f, 0f);
                    tmp.scale(t, v);
                    nearest.add(from, tmp);
                    tmp.sub(simplexPointsP[1], simplexPointsP[0]);
                    tmp.scale(t);
                    cachedP1.add(simplexPointsP[0], tmp);
                    tmp.sub(simplexPointsQ[1], simplexPointsQ[0]);
                    tmp.scale(t);
                    cachedP2.add(simplexPointsQ[0], tmp);
                    cachedV.sub(cachedP1, cachedP2);
                    reduceVertices(cachedBC.usedVertices);
                    cachedValidClosest = cachedBC.isValid();
                    break;
                }
            case 3:
                {
                    Vector3f tmp1 = stack.allocVector3f();
                    Vector3f tmp2 = stack.allocVector3f();
                    Vector3f tmp3 = stack.allocVector3f();
                    // closest point origin from triangle 
                    Vector3f p = stack.allocVector3f();
                    p.set(0f, 0f, 0f);
                    Vector3f a = simplexVectorW[0];
                    Vector3f b = simplexVectorW[1];
                    Vector3f c = simplexVectorW[2];
                    closestPtPointTriangle(p, a, b, c, cachedBC);
                    tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsP[0]);
                    tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsP[1]);
                    tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsP[2]);
                    VectorUtil.add(cachedP1, tmp1, tmp2, tmp3);
                    tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsQ[0]);
                    tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsQ[1]);
                    tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsQ[2]);
                    VectorUtil.add(cachedP2, tmp1, tmp2, tmp3);
                    cachedV.sub(cachedP1, cachedP2);
                    reduceVertices(cachedBC.usedVertices);
                    cachedValidClosest = cachedBC.isValid();
                    break;
                }
            case 4:
                {
                    Vector3f tmp1 = stack.allocVector3f();
                    Vector3f tmp2 = stack.allocVector3f();
                    Vector3f tmp3 = stack.allocVector3f();
                    Vector3f tmp4 = stack.allocVector3f();
                    Vector3f p = stack.allocVector3f();
                    p.set(0f, 0f, 0f);
                    Vector3f a = simplexVectorW[0];
                    Vector3f b = simplexVectorW[1];
                    Vector3f c = simplexVectorW[2];
                    Vector3f d = simplexVectorW[3];
                    boolean hasSeperation = closestPtPointTetrahedron(p, a, b, c, d, cachedBC);
                    if (hasSeperation) {
                        tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsP[0]);
                        tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsP[1]);
                        tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsP[2]);
                        tmp4.scale(cachedBC.barycentricCoords[3], simplexPointsP[3]);
                        VectorUtil.add(cachedP1, tmp1, tmp2, tmp3, tmp4);
                        tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsQ[0]);
                        tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsQ[1]);
                        tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsQ[2]);
                        tmp4.scale(cachedBC.barycentricCoords[3], simplexPointsQ[3]);
                        VectorUtil.add(cachedP2, tmp1, tmp2, tmp3, tmp4);
                        cachedV.sub(cachedP1, cachedP2);
                        reduceVertices(cachedBC.usedVertices);
                    } else {
                        if (cachedBC.degenerate) {
                            cachedValidClosest = false;
                        } else {
                            cachedValidClosest = true;
                            //degenerate case == false, penetration = true + zero
                            cachedV.set(0f, 0f, 0f);
                        }
                        break;
                    }
                    cachedValidClosest = cachedBC.isValid();
                    //closest point origin from tetrahedron
                    break;
                }
            default:
                {
                    cachedValidClosest = false;
                }
        }
        stack.leave();
    }
    return cachedValidClosest;
}

Example 4 with StaticAlloc

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

the class RigidBody method applyImpulse.

@StaticAlloc
public void applyImpulse(Vector3f impulse, Vector3f rel_pos) {
    if (inverseMass != 0f) {
        applyCentralImpulse(impulse);
        if (angularFactor != 0f) {
            Stack stack = Stack.enter();
            Vector3f tmp = stack.allocVector3f();
            tmp.cross(rel_pos, impulse);
            tmp.scale(angularFactor);
            applyTorqueImpulse(tmp);
            stack.leave();
        }
    }
}

Example 5 with StaticAlloc

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

the class Transform method mul.

@StaticAlloc
public void mul(Transform tr1, Transform tr2) {
    Stack stack = Stack.enter();
    Vector3f vec = stack.alloc(tr2.origin);
    tr1.transform(vec);
    basis.mul(tr1.basis, tr2.basis);
    origin.set(vec);
    stack.leave();
}