Examples with TempVars com.jme3.util.TempVars used on opensource projects

Example 51 with TempVars

use of com.jme3.util.TempVars in project jmonkeyengine by jMonkeyEngine.

the class BoundingSphere method collideWithTri.

private int collideWithTri(Triangle tri, CollisionResults results) {
    TempVars tvars = TempVars.get();
    try {
        // Much of this is based on adaptation from this algorithm:
        // http://realtimecollisiondetection.net/blog/?p=103
        // ...mostly the stuff about eliminating sqrts wherever
        // possible.
        // Math is done in center-relative space.
        Vector3f a = tri.get1().subtract(center, tvars.vect1);
        Vector3f b = tri.get2().subtract(center, tvars.vect2);
        Vector3f c = tri.get3().subtract(center, tvars.vect3);
        Vector3f ab = b.subtract(a, tvars.vect4);
        Vector3f ac = c.subtract(a, tvars.vect5);
        // Check the plane... if it doesn't intersect the plane
        // then it doesn't intersect the triangle.
        Vector3f n = ab.cross(ac, tvars.vect6);
        float d = a.dot(n);
        float e = n.dot(n);
        if (d * d > radius * radius * e) {
            // Can't possibly intersect
            return 0;
        }
        // We intersect the verts, or the edges, or the face...
        // First check against the face since it's the most
        // specific.
        // Calculate the barycentric coordinates of the
        // sphere center
        Vector3f v0 = ac;
        Vector3f v1 = ab;
        // a was P relative, so p.subtract(a) is just -a
        // instead of wasting a vector we'll just negate the
        // dot products below... it's all v2 is used for.
        Vector3f v2 = a;
        float dot00 = v0.dot(v0);
        float dot01 = v0.dot(v1);
        float dot02 = -v0.dot(v2);
        float dot11 = v1.dot(v1);
        float dot12 = -v1.dot(v2);
        float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
        float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
        float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
        if (u >= 0 && v >= 0 && (u + v) <= 1) {
            // We intersect... and we even know where
            Vector3f part1 = ac;
            Vector3f part2 = ab;
            Vector3f p = center.add(a.add(part1.mult(u)).addLocal(part2.mult(v)));
            CollisionResult r = new CollisionResult();
            Vector3f normal = n.normalize();
            // a is center relative, so -a points to center
            float dist = -normal.dot(a);
            dist = dist - radius;
            r.setDistance(dist);
            r.setContactNormal(normal);
            r.setContactPoint(p);
            results.addCollision(r);
            return 1;
        }
        // Check the edges looking for the nearest point
        // that is also less than the radius.  We don't care
        // about points that are farther away than that.
        Vector3f nearestPt = null;
        float nearestDist = radius * radius;
        Vector3f base;
        Vector3f edge;
        float t;
        // Edge AB
        base = a;
        edge = ab;
        t = -edge.dot(base) / edge.dot(edge);
        if (t >= 0 && t <= 1) {
            Vector3f Q = base.add(edge.mult(t, tvars.vect7), tvars.vect8);
            // distance squared to origin
            float distSq = Q.dot(Q);
            if (distSq < nearestDist) {
                nearestPt = Q;
                nearestDist = distSq;
            }
        }
        // Edge AC
        base = a;
        edge = ac;
        t = -edge.dot(base) / edge.dot(edge);
        if (t >= 0 && t <= 1) {
            Vector3f Q = base.add(edge.mult(t, tvars.vect7), tvars.vect9);
            // distance squared to origin
            float distSq = Q.dot(Q);
            if (distSq < nearestDist) {
                nearestPt = Q;
                nearestDist = distSq;
            }
        }
        // Edge BC
        base = b;
        Vector3f bc = c.subtract(b);
        edge = bc;
        t = -edge.dot(base) / edge.dot(edge);
        if (t >= 0 && t <= 1) {
            Vector3f Q = base.add(edge.mult(t, tvars.vect7), tvars.vect10);
            // distance squared to origin
            float distSq = Q.dot(Q);
            if (distSq < nearestDist) {
                nearestPt = Q;
                nearestDist = distSq;
            }
        }
        // done.       
        if (nearestPt != null) {
            // We have a hit
            float dist = FastMath.sqrt(nearestDist);
            Vector3f cn = nearestPt.divide(-dist);
            CollisionResult r = new CollisionResult();
            r.setDistance(dist - radius);
            r.setContactNormal(cn);
            r.setContactPoint(nearestPt.add(center));
            results.addCollision(r);
            return 1;
        }
        // Finally check each of the triangle corners
        // Vert A
        base = a;
        // distance squared to origin
        t = base.dot(base);
        if (t < nearestDist) {
            nearestDist = t;
            nearestPt = base;
        }
        // Vert B
        base = b;
        // distance squared to origin
        t = base.dot(base);
        if (t < nearestDist) {
            nearestDist = t;
            nearestPt = base;
        }
        // Vert C
        base = c;
        // distance squared to origin
        t = base.dot(base);
        if (t < nearestDist) {
            nearestDist = t;
            nearestPt = base;
        }
        if (nearestPt != null) {
            // We have a hit
            float dist = FastMath.sqrt(nearestDist);
            Vector3f cn = nearestPt.divide(-dist);
            CollisionResult r = new CollisionResult();
            r.setDistance(dist - radius);
            r.setContactNormal(cn);
            r.setContactPoint(nearestPt.add(center));
            results.addCollision(r);
            return 1;
        }
        // Nothing hit... oh, well 
        return 0;
    } finally {
        tvars.release();
    }
}

Example 52 with TempVars

use of com.jme3.util.TempVars in project jmonkeyengine by jMonkeyEngine.

the class BoundingVolume method collideWith.

public int collideWith(Collidable other) {
    TempVars tempVars = TempVars.get();
    try {
        CollisionResults tempResults = tempVars.collisionResults;
        tempResults.clear();
        return collideWith(other, tempResults);
    } finally {
        tempVars.release();
    }
}

Example 53 with TempVars

use of com.jme3.util.TempVars in project jmonkeyengine by jMonkeyEngine.

the class Intersection method intersect.

//    private boolean axisTest(float a, float b, float fa, float fb, Vector3f v0, Vector3f v1, )
//    private boolean axisTestX01(float a, float b, float fa, float fb,
//                             Vector3f center, Vector3f ext,
//                             Vector3f v1, Vector3f v2, Vector3f v3){
//	float p0 = a * v0.y - b * v0.z;
//	float p2 = a * v2.y - b * v2.z;
//        if(p0 < p2){
//            min = p0;
//            max = p2;
//        } else {
//            min = p2;
//            max = p0;
//        }
//	float rad = fa * boxhalfsize.y + fb * boxhalfsize.z;
//	if(min > rad || max < -rad)
//            return false;
//    }
public static boolean intersect(BoundingBox bbox, Vector3f v1, Vector3f v2, Vector3f v3) {
    //  use separating axis theorem to test overlap between triangle and box
    //  need to test for overlap in these directions:
    //  1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
    //     we do not even need to test these)
    //  2) normal of the triangle
    //  3) crossproduct(edge from tri, {x,y,z}-directin)
    //       this gives 3x3=9 more tests
    TempVars vars = TempVars.get();
    Vector3f tmp0 = vars.vect1, tmp1 = vars.vect2, tmp2 = vars.vect3;
    Vector3f e0 = vars.vect4, e1 = vars.vect5, e2 = vars.vect6;
    Vector3f center = bbox.getCenter();
    Vector3f extent = bbox.getExtent(null);
    //   float min,max,p0,p1,p2,rad,fex,fey,fez;
    //   float normal[3]
    // This is the fastest branch on Sun
    // move everything so that the boxcenter is in (0,0,0)
    v1.subtract(center, tmp0);
    v2.subtract(center, tmp1);
    v3.subtract(center, tmp2);
    // compute triangle edges
    // tri edge 0
    tmp1.subtract(tmp0, e0);
    // tri edge 1
    tmp2.subtract(tmp1, e1);
    // tri edge 2
    tmp0.subtract(tmp2, e2);
    // Bullet 3:
    //  test the 9 tests first (this was faster)
    float min, max;
    float p0, p1, p2, rad;
    float fex = FastMath.abs(e0.x);
    float fey = FastMath.abs(e0.y);
    float fez = FastMath.abs(e0.z);
    //AXISTEST_X01(e0[Z], e0[Y], fez, fey);
    p0 = e0.z * tmp0.y - e0.y * tmp0.z;
    p2 = e0.z * tmp2.y - e0.y * tmp2.z;
    min = min(p0, p2);
    max = max(p0, p2);
    rad = fez * extent.y + fey * extent.z;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    //   AXISTEST_Y02(e0[Z], e0[X], fez, fex);
    p0 = -e0.z * tmp0.x + e0.x * tmp0.z;
    p2 = -e0.z * tmp2.x + e0.x * tmp2.z;
    min = min(p0, p2);
    max = max(p0, p2);
    rad = fez * extent.x + fex * extent.z;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    // AXISTEST_Z12(e0[Y], e0[X], fey, fex);
    p1 = e0.y * tmp1.x - e0.x * tmp1.y;
    p2 = e0.y * tmp2.x - e0.x * tmp2.y;
    min = min(p1, p2);
    max = max(p1, p2);
    rad = fey * extent.x + fex * extent.y;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    fex = FastMath.abs(e1.x);
    fey = FastMath.abs(e1.y);
    fez = FastMath.abs(e1.z);
    //        AXISTEST_X01(e1[Z], e1[Y], fez, fey);
    p0 = e1.z * tmp0.y - e1.y * tmp0.z;
    p2 = e1.z * tmp2.y - e1.y * tmp2.z;
    min = min(p0, p2);
    max = max(p0, p2);
    rad = fez * extent.y + fey * extent.z;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    //   AXISTEST_Y02(e1[Z], e1[X], fez, fex);
    p0 = -e1.z * tmp0.x + e1.x * tmp0.z;
    p2 = -e1.z * tmp2.x + e1.x * tmp2.z;
    min = min(p0, p2);
    max = max(p0, p2);
    rad = fez * extent.x + fex * extent.z;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    // AXISTEST_Z0(e1[Y], e1[X], fey, fex);
    p0 = e1.y * tmp0.x - e1.x * tmp0.y;
    p1 = e1.y * tmp1.x - e1.x * tmp1.y;
    min = min(p0, p1);
    max = max(p0, p1);
    rad = fey * extent.x + fex * extent.y;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    //
    fex = FastMath.abs(e2.x);
    fey = FastMath.abs(e2.y);
    fez = FastMath.abs(e2.z);
    // AXISTEST_X2(e2[Z], e2[Y], fez, fey);
    p0 = e2.z * tmp0.y - e2.y * tmp0.z;
    p1 = e2.z * tmp1.y - e2.y * tmp1.z;
    min = min(p0, p1);
    max = max(p0, p1);
    rad = fez * extent.y + fey * extent.z;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    // AXISTEST_Y1(e2[Z], e2[X], fez, fex);
    p0 = -e2.z * tmp0.x + e2.x * tmp0.z;
    p1 = -e2.z * tmp1.x + e2.x * tmp1.z;
    min = min(p0, p1);
    max = max(p0, p1);
    rad = fez * extent.x + fex * extent.y;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    //   AXISTEST_Z12(e2[Y], e2[X], fey, fex);
    p1 = e2.y * tmp1.x - e2.x * tmp1.y;
    p2 = e2.y * tmp2.x - e2.x * tmp2.y;
    min = min(p1, p2);
    max = max(p1, p2);
    rad = fey * extent.x + fex * extent.y;
    if (min > rad || max < -rad) {
        vars.release();
        return false;
    }
    //  Bullet 1:
    //  first test overlap in the {x,y,z}-directions
    //  find min, max of the triangle each direction, and test for overlap in
    //  that direction -- this is equivalent to testing a minimal AABB around
    //  the triangle against the AABB
    Vector3f minMax = vars.vect7;
    // test in X-direction
    findMinMax(tmp0.x, tmp1.x, tmp2.x, minMax);
    if (minMax.x > extent.x || minMax.y < -extent.x) {
        vars.release();
        return false;
    }
    // test in Y-direction
    findMinMax(tmp0.y, tmp1.y, tmp2.y, minMax);
    if (minMax.x > extent.y || minMax.y < -extent.y) {
        vars.release();
        return false;
    }
    // test in Z-direction
    findMinMax(tmp0.z, tmp1.z, tmp2.z, minMax);
    if (minMax.x > extent.z || minMax.y < -extent.z) {
        vars.release();
        return false;
    }
    //       // Bullet 2:
    //       //  test if the box intersects the plane of the triangle
    //       //  compute plane equation of triangle: normal * x + d = 0
    //        Vector3f normal = new Vector3f();
    //        e0.cross(e1, normal);
    Plane p = vars.plane;
    p.setPlanePoints(v1, v2, v3);
    if (bbox.whichSide(p) == Plane.Side.Negative) {
        vars.release();
        return false;
    }
    //
    //        if(!planeBoxOverlap(normal,v0,boxhalfsize)) return false;
    vars.release();
    return true;
/* box and triangle overlaps */
}

Example 54 with TempVars

use of com.jme3.util.TempVars in project jmonkeyengine by jMonkeyEngine.

the class BIHNode method intersectWhere.

public final int intersectWhere(Ray r, Matrix4f worldMatrix, BIHTree tree, float sceneMin, float sceneMax, CollisionResults results) {
    TempVars vars = TempVars.get();
    ArrayList<BIHStackData> stack = vars.bihStack;
    stack.clear();
    //        float tHit = Float.POSITIVE_INFINITY;
    Vector3f o = vars.vect1.set(r.getOrigin());
    Vector3f d = vars.vect2.set(r.getDirection());
    Matrix4f inv = vars.tempMat4.set(worldMatrix).invertLocal();
    inv.mult(r.getOrigin(), r.getOrigin());
    // Fixes rotation collision bug
    inv.multNormal(r.getDirection(), r.getDirection());
    //        inv.multNormalAcross(r.getDirection(), r.getDirection());
    float[] origins = { r.getOrigin().x, r.getOrigin().y, r.getOrigin().z };
    float[] invDirections = { 1f / r.getDirection().x, 1f / r.getDirection().y, 1f / r.getDirection().z };
    r.getDirection().normalizeLocal();
    Vector3f v1 = vars.vect3, v2 = vars.vect4, v3 = vars.vect5;
    int cols = 0;
    stack.add(new BIHStackData(this, sceneMin, sceneMax));
    stackloop: while (stack.size() > 0) {
        BIHStackData data = stack.remove(stack.size() - 1);
        BIHNode node = data.node;
        float tMin = data.min, tMax = data.max;
        if (tMax < tMin) {
            continue;
        }
        leafloop: while (node.axis != 3) {
            // while node is not a leaf
            int a = node.axis;
            // find the origin and direction value for the given axis
            float origin = origins[a];
            float invDirection = invDirections[a];
            float tNearSplit, tFarSplit;
            BIHNode nearNode, farNode;
            tNearSplit = (node.leftPlane - origin) * invDirection;
            tFarSplit = (node.rightPlane - origin) * invDirection;
            nearNode = node.left;
            farNode = node.right;
            if (invDirection < 0) {
                float tmpSplit = tNearSplit;
                tNearSplit = tFarSplit;
                tFarSplit = tmpSplit;
                BIHNode tmpNode = nearNode;
                nearNode = farNode;
                farNode = tmpNode;
            }
            if (tMin > tNearSplit && tMax < tFarSplit) {
                continue stackloop;
            }
            if (tMin > tNearSplit) {
                tMin = max(tMin, tFarSplit);
                node = farNode;
            } else if (tMax < tFarSplit) {
                tMax = min(tMax, tNearSplit);
                node = nearNode;
            } else {
                stack.add(new BIHStackData(farNode, max(tMin, tFarSplit), tMax));
                tMax = min(tMax, tNearSplit);
                node = nearNode;
            }
        }
        // a leaf
        for (int i = node.leftIndex; i <= node.rightIndex; i++) {
            tree.getTriangle(i, v1, v2, v3);
            float t = r.intersects(v1, v2, v3);
            if (!Float.isInfinite(t)) {
                if (worldMatrix != null) {
                    worldMatrix.mult(v1, v1);
                    worldMatrix.mult(v2, v2);
                    worldMatrix.mult(v3, v3);
                    float t_world = new Ray(o, d).intersects(v1, v2, v3);
                    t = t_world;
                }
                Vector3f contactNormal = Triangle.computeTriangleNormal(v1, v2, v3, null);
                Vector3f contactPoint = new Vector3f(d).multLocal(t).addLocal(o);
                float worldSpaceDist = o.distance(contactPoint);
                CollisionResult cr = new CollisionResult(contactPoint, worldSpaceDist);
                cr.setContactNormal(contactNormal);
                cr.setTriangleIndex(tree.getTriangleIndex(i));
                results.addCollision(cr);
                cols++;
            }
        }
    }
    vars.release();
    r.setOrigin(o);
    r.setDirection(d);
    return cols;
}

Example 55 with TempVars

use of com.jme3.util.TempVars in project jmonkeyengine by jMonkeyEngine.

the class BIHNode method intersectBrute.

public final int intersectBrute(Ray r, Matrix4f worldMatrix, BIHTree tree, float sceneMin, float sceneMax, CollisionResults results) {
    float tHit = Float.POSITIVE_INFINITY;
    TempVars vars = TempVars.get();
    Vector3f v1 = vars.vect1, v2 = vars.vect2, v3 = vars.vect3;
    int cols = 0;
    ArrayList<BIHStackData> stack = vars.bihStack;
    stack.clear();
    stack.add(new BIHStackData(this, 0, 0));
    stackloop: while (stack.size() > 0) {
        BIHStackData data = stack.remove(stack.size() - 1);
        BIHNode node = data.node;
        leafloop: while (node.axis != 3) {
            // while node is not a leaf
            BIHNode nearNode, farNode;
            nearNode = node.left;
            farNode = node.right;
            stack.add(new BIHStackData(farNode, 0, 0));
            node = nearNode;
        }
        // a leaf
        for (int i = node.leftIndex; i <= node.rightIndex; i++) {
            tree.getTriangle(i, v1, v2, v3);
            if (worldMatrix != null) {
                worldMatrix.mult(v1, v1);
                worldMatrix.mult(v2, v2);
                worldMatrix.mult(v3, v3);
            }
            float t = r.intersects(v1, v2, v3);
            if (t < tHit) {
                tHit = t;
                Vector3f contactPoint = new Vector3f(r.direction).multLocal(tHit).addLocal(r.origin);
                CollisionResult cr = new CollisionResult(contactPoint, tHit);
                cr.setTriangleIndex(tree.getTriangleIndex(i));
                results.addCollision(cr);
                cols++;
            }
        }
    }
    vars.release();
    return cols;
}