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

Example 31 with Stack

use of com.bulletphysics.util.Stack 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 32 with Stack

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

the class BoxShape method batchedUnitVectorGetSupportingVertexWithoutMargin.

@Override
public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
    Stack stack = Stack.enter();
    Vector3f halfExtents = getHalfExtentsWithoutMargin(stack.allocVector3f());
    for (int i = 0; i < numVectors; i++) {
        Vector3f vec = vectors[i];
        supportVerticesOut[i].set(ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x), ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y), ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z));
    }
    stack.leave();
}

Example 33 with Stack

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

the class BoxShape method getVertex.

@Override
public void getVertex(int i, Vector3f vtx) {
    Stack stack = Stack.enter();
    Vector3f halfExtents = getHalfExtentsWithoutMargin(stack.allocVector3f());
    vtx.set(halfExtents.x * (1 - (i & 1)) - halfExtents.x * (i & 1), halfExtents.y * (1 - ((i & 2) >> 1)) - halfExtents.y * ((i & 2) >> 1), halfExtents.z * (1 - ((i & 4) >> 2)) - halfExtents.z * ((i & 4) >> 2));
    stack.leave();
}

Example 34 with Stack

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

the class BoxShape method setMargin.

@Override
public void setMargin(float margin) {
    Stack stack = Stack.enter();
    // correct the implicitShapeDimensions for the margin
    Vector3f oldMargin = stack.allocVector3f();
    oldMargin.set(getMargin(), getMargin(), getMargin());
    Vector3f implicitShapeDimensionsWithMargin = stack.allocVector3f();
    implicitShapeDimensionsWithMargin.add(implicitShapeDimensions, oldMargin);
    super.setMargin(margin);
    Vector3f newMargin = stack.allocVector3f();
    newMargin.set(getMargin(), getMargin(), getMargin());
    implicitShapeDimensions.sub(implicitShapeDimensionsWithMargin, newMargin);
    stack.leave();
}

Example 35 with Stack

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

the class BoxShape method setLocalScaling.

@Override
public void setLocalScaling(Vector3f scaling) {
    Stack stack = Stack.enter();
    Vector3f oldMargin = stack.allocVector3f();
    oldMargin.set(getMargin(), getMargin(), getMargin());
    Vector3f implicitShapeDimensionsWithMargin = stack.allocVector3f();
    implicitShapeDimensionsWithMargin.add(implicitShapeDimensions, oldMargin);
    Vector3f unScaledImplicitShapeDimensionsWithMargin = stack.allocVector3f();
    VectorUtil.div(unScaledImplicitShapeDimensionsWithMargin, implicitShapeDimensionsWithMargin, localScaling);
    super.setLocalScaling(scaling);
    VectorUtil.mul(implicitShapeDimensions, unScaledImplicitShapeDimensionsWithMargin, localScaling);
    implicitShapeDimensions.sub(oldMargin);
    stack.leave();
}