Examples with Vector3f javax.vecmath.Vector3f used on opensource projects

Example 31 with Vector3f

use of javax.vecmath.Vector3f 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 32 with Vector3f

use of javax.vecmath.Vector3f 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 33 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class BU_Simplex1to4 method addVertex.

public void addVertex(Vector3f pt) {
    if (vertices[numVertices] == null) {
        vertices[numVertices] = new Vector3f();
    }
    vertices[numVertices++] = pt;
    recalcLocalAabb();
}

Example 34 with Vector3f

use of javax.vecmath.Vector3f 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 35 with Vector3f

use of javax.vecmath.Vector3f in project bdx by GoranM.

the class BoxShape method localGetSupportingVertex.

@Override
public Vector3f localGetSupportingVertex(Vector3f vec, Vector3f out) {
    Vector3f halfExtents = getHalfExtentsWithoutMargin(out);
    float margin = getMargin();
    halfExtents.x += margin;
    halfExtents.y += margin;
    halfExtents.z += margin;
    out.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));
    return out;
}