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

Example 26 with Vector3f

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

the class PersistentManifold method getCacheEntry.

public int getCacheEntry(ManifoldPoint newPoint) {
    float shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
    int size = getNumContacts();
    int nearestPoint = -1;
    Stack stack = Stack.enter();
    Vector3f diffA = stack.allocVector3f();
    for (int i = 0; i < size; i++) {
        ManifoldPoint mp = pointCache[i];
        diffA.sub(mp.localPointA, newPoint.localPointA);
        float distToManiPoint = diffA.dot(diffA);
        if (distToManiPoint < shortestDist) {
            shortestDist = distToManiPoint;
            nearestPoint = i;
        }
    }
    stack.leave();
    return nearestPoint;
}

Example 27 with Vector3f

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

the class PersistentManifold method sortCachedPoints.

/// sort cached points so most isolated points come first
private int sortCachedPoints(ManifoldPoint pt) {
    //calculate 4 possible cases areas, and take biggest area
    //also need to keep 'deepest'
    int maxPenetrationIndex = -1;
    //#define KEEP_DEEPEST_POINT 1
    //#ifdef KEEP_DEEPEST_POINT
    float maxPenetration = pt.getDistance();
    for (int i = 0; i < 4; i++) {
        if (pointCache[i].getDistance() < maxPenetration) {
            maxPenetrationIndex = i;
            maxPenetration = pointCache[i].getDistance();
        }
    }
    //#endif //KEEP_DEEPEST_POINT
    Stack stack = Stack.enter();
    float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f;
    if (maxPenetrationIndex != 0) {
        Vector3f a0 = stack.alloc(pt.localPointA);
        a0.sub(pointCache[1].localPointA);
        Vector3f b0 = stack.alloc(pointCache[3].localPointA);
        b0.sub(pointCache[2].localPointA);
        Vector3f cross = stack.allocVector3f();
        cross.cross(a0, b0);
        res0 = cross.lengthSquared();
    }
    if (maxPenetrationIndex != 1) {
        Vector3f a1 = stack.alloc(pt.localPointA);
        a1.sub(pointCache[0].localPointA);
        Vector3f b1 = stack.alloc(pointCache[3].localPointA);
        b1.sub(pointCache[2].localPointA);
        Vector3f cross = stack.allocVector3f();
        cross.cross(a1, b1);
        res1 = cross.lengthSquared();
    }
    if (maxPenetrationIndex != 2) {
        Vector3f a2 = stack.alloc(pt.localPointA);
        a2.sub(pointCache[0].localPointA);
        Vector3f b2 = stack.alloc(pointCache[3].localPointA);
        b2.sub(pointCache[1].localPointA);
        Vector3f cross = stack.allocVector3f();
        cross.cross(a2, b2);
        res2 = cross.lengthSquared();
    }
    if (maxPenetrationIndex != 3) {
        Vector3f a3 = stack.alloc(pt.localPointA);
        a3.sub(pointCache[0].localPointA);
        Vector3f b3 = stack.alloc(pointCache[2].localPointA);
        b3.sub(pointCache[1].localPointA);
        Vector3f cross = stack.allocVector3f();
        cross.cross(a3, b3);
        res3 = cross.lengthSquared();
    }
    Vector4f maxvec = stack.allocVector4f();
    maxvec.set(res0, res1, res2, res3);
    int biggestarea = VectorUtil.closestAxis4(maxvec);
    stack.leave();
    return biggestarea;
}

Example 28 with Vector3f

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

the class SubsimplexConvexCast method calcTimeOfImpact.

public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
    Stack stack = Stack.enter();
    Vector3f tmp = stack.allocVector3f();
    simplexSolver.reset();
    Vector3f linVelA = stack.allocVector3f();
    Vector3f linVelB = stack.allocVector3f();
    linVelA.sub(toA.origin, fromA.origin);
    linVelB.sub(toB.origin, fromB.origin);
    float lambda = 0f;
    Transform interpolatedTransA = stack.alloc(fromA);
    Transform interpolatedTransB = stack.alloc(fromB);
    // take relative motion
    Vector3f r = stack.allocVector3f();
    r.sub(linVelA, linVelB);
    Vector3f v = stack.allocVector3f();
    tmp.negate(r);
    MatrixUtil.transposeTransform(tmp, tmp, fromA.basis);
    Vector3f supVertexA = convexA.localGetSupportingVertex(tmp, stack.allocVector3f());
    fromA.transform(supVertexA);
    MatrixUtil.transposeTransform(tmp, r, fromB.basis);
    Vector3f supVertexB = convexB.localGetSupportingVertex(tmp, stack.allocVector3f());
    fromB.transform(supVertexB);
    v.sub(supVertexA, supVertexB);
    int maxIter = MAX_ITERATIONS;
    Vector3f n = stack.allocVector3f();
    n.set(0f, 0f, 0f);
    boolean hasResult = false;
    Vector3f c = stack.allocVector3f();
    float lastLambda = lambda;
    float dist2 = v.lengthSquared();
    //#ifdef BT_USE_DOUBLE_PRECISION
    //	btScalar epsilon = btScalar(0.0001);
    //#else
    float epsilon = 0.0001f;
    //#endif
    Vector3f w = stack.allocVector3f(), p = stack.allocVector3f();
    float VdotR;
    while ((dist2 > epsilon) && (maxIter--) != 0) {
        tmp.negate(v);
        MatrixUtil.transposeTransform(tmp, tmp, interpolatedTransA.basis);
        convexA.localGetSupportingVertex(tmp, supVertexA);
        interpolatedTransA.transform(supVertexA);
        MatrixUtil.transposeTransform(tmp, v, interpolatedTransB.basis);
        convexB.localGetSupportingVertex(tmp, supVertexB);
        interpolatedTransB.transform(supVertexB);
        w.sub(supVertexA, supVertexB);
        float VdotW = v.dot(w);
        if (lambda > 1f) {
            stack.leave();
            return false;
        }
        if (VdotW > 0f) {
            VdotR = v.dot(r);
            if (VdotR >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
                stack.leave();
                return false;
            } else {
                lambda = lambda - VdotW / VdotR;
                // interpolate to next lambda
                //	x = s + lambda * r;
                VectorUtil.setInterpolate3(interpolatedTransA.origin, fromA.origin, toA.origin, lambda);
                VectorUtil.setInterpolate3(interpolatedTransB.origin, fromB.origin, toB.origin, lambda);
                //m_simplexSolver->reset();
                // check next line
                w.sub(supVertexA, supVertexB);
                lastLambda = lambda;
                n.set(v);
                hasResult = true;
            }
        }
        simplexSolver.addVertex(w, supVertexA, supVertexB);
        if (simplexSolver.closest(v)) {
            dist2 = v.lengthSquared();
            hasResult = true;
        // todo: check this normal for validity
        //n.set(v);
        //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
        //printf("DIST2=%f\n",dist2);
        //printf("numverts = %i\n",m_simplexSolver->numVertices());
        } else {
            dist2 = 0f;
        }
    }
    //int numiter = MAX_ITERATIONS - maxIter;
    //	printf("number of iterations: %d", numiter);
    // don't report a time of impact when moving 'away' from the hitnormal
    result.fraction = lambda;
    if (n.lengthSquared() >= BulletGlobals.SIMD_EPSILON * BulletGlobals.SIMD_EPSILON) {
        result.normal.normalize(n);
    } else {
        result.normal.set(0f, 0f, 0f);
    }
    // don't report time of impact for motion away from the contact normal (or causes minor penetration)
    if (result.normal.dot(r) >= -result.allowedPenetration) {
        stack.leave();
        return false;
    }
    Vector3f hitA = stack.allocVector3f();
    Vector3f hitB = stack.allocVector3f();
    simplexSolver.compute_points(hitA, hitB);
    result.hitPoint.set(hitB);
    stack.leave();
    return true;
}

Example 29 with Vector3f

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

the class TriangleRaycastCallback method processTriangle.

public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
    Stack stack = Stack.enter();
    Vector3f vert0 = triangle[0];
    Vector3f vert1 = triangle[1];
    Vector3f vert2 = triangle[2];
    Vector3f v10 = stack.allocVector3f();
    v10.sub(vert1, vert0);
    Vector3f v20 = stack.allocVector3f();
    v20.sub(vert2, vert0);
    Vector3f triangleNormal = stack.allocVector3f();
    triangleNormal.cross(v10, v20);
    float dist = vert0.dot(triangleNormal);
    float dist_a = triangleNormal.dot(from);
    dist_a -= dist;
    float dist_b = triangleNormal.dot(to);
    dist_b -= dist;
    if (dist_a * dist_b >= 0f) {
        stack.leave();
        // same sign
        return;
    }
    float proj_length = dist_a - dist_b;
    float distance = (dist_a) / (proj_length);
    if (distance < hitFraction) {
        float edge_tolerance = triangleNormal.lengthSquared();
        edge_tolerance *= -0.0001f;
        Vector3f point = new Vector3f();
        VectorUtil.setInterpolate3(point, from, to, distance);
        {
            Vector3f v0p = stack.allocVector3f();
            v0p.sub(vert0, point);
            Vector3f v1p = stack.allocVector3f();
            v1p.sub(vert1, point);
            Vector3f cp0 = stack.allocVector3f();
            cp0.cross(v0p, v1p);
            if (cp0.dot(triangleNormal) >= edge_tolerance) {
                Vector3f v2p = stack.allocVector3f();
                v2p.sub(vert2, point);
                Vector3f cp1 = stack.allocVector3f();
                cp1.cross(v1p, v2p);
                if (cp1.dot(triangleNormal) >= edge_tolerance) {
                    Vector3f cp2 = stack.allocVector3f();
                    cp2.cross(v2p, v0p);
                    if (cp2.dot(triangleNormal) >= edge_tolerance) {
                        if (dist_a > 0f) {
                            hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex);
                        } else {
                            Vector3f tmp = stack.allocVector3f();
                            tmp.negate(triangleNormal);
                            hitFraction = reportHit(tmp, distance, partId, triangleIndex);
                        }
                    }
                }
            }
        }
    }
    stack.leave();
}

Example 30 with Vector3f

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