All Downloads are FREE. Search and download functionalities are using the official Maven repository.

com.jme3.collision.SweepSphere Maven / Gradle / Ivy

There is a newer version: 3.7.0-stable
Show newest version
/*
 * Copyright (c) 2009-2021 jMonkeyEngine
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 * * Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the distribution.
 *
 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors
 *   may be used to endorse or promote products derived from this software
 *   without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package com.jme3.collision;

import com.jme3.math.*;

/**
 * No longer public.
 *
 * @author Kirill Vainer
 */
@Deprecated
class SweepSphere implements Collidable {

    final private Vector3f velocity = new Vector3f();
    final private Vector3f center = new Vector3f();
    final private Vector3f dimension = new Vector3f();
    final private Vector3f invDim = new Vector3f();

    private final Triangle scaledTri = new Triangle();
    private final Plane triPlane = new Plane();
    private final Vector3f temp1 = new Vector3f(),
                           temp2 = new Vector3f(),
                           temp3 = new Vector3f();
    private final Vector3f sVelocity = new Vector3f(),
                           sCenter = new Vector3f();

    public Vector3f getCenter() {
        return center;
    }

    public void setCenter(Vector3f center) {
        this.center.set(center);
    }

    public Vector3f getDimension() {
        return dimension;
    }

    public void setDimension(Vector3f dimension) {
        this.dimension.set(dimension);
        this.invDim.set(1, 1, 1).divideLocal(dimension);
    }

    public void setDimension(float x, float y, float z) {
        this.dimension.set(x, y, z);
        this.invDim.set(1, 1, 1).divideLocal(dimension);
    }

    public void setDimension(float dim) {
        this.dimension.set(dim, dim, dim);
        this.invDim.set(1, 1, 1).divideLocal(dimension);
    }

    public Vector3f getVelocity() {
        return velocity;
    }

    public void setVelocity(Vector3f velocity) {
        this.velocity.set(velocity);
    }

    private boolean pointsOnSameSide(Vector3f p1, Vector3f p2, Vector3f line1, Vector3f line2) {
        // V1 = (line2 - line1) x (p1    - line1)
        // V2 = (p2    - line1) x (line2 - line1)

        temp1.set(line2).subtractLocal(line1);
        temp3.set(temp1);
        temp2.set(p1).subtractLocal(line1);
        temp1.crossLocal(temp2);

        temp2.set(p2).subtractLocal(line1);
        temp3.crossLocal(temp2);

        // V1 . V2 >= 0
        return temp1.dot(temp3) >= 0;
    }

    private boolean isPointInTriangle(Vector3f point, AbstractTriangle tri) {
        if (pointsOnSameSide(point, tri.get1(), tri.get2(), tri.get3())
                && pointsOnSameSide(point, tri.get2(), tri.get1(), tri.get3())
                && pointsOnSameSide(point, tri.get3(), tri.get1(), tri.get2())) {
            return true;
        }
        return false;
    }

    private static float getLowestRoot(float a, float b, float c, float maxR) {
        float determinant = b * b - 4f * a * c;
        if (determinant < 0) {
            return Float.NaN;
        }

        float sqrtDet = FastMath.sqrt(determinant);
        float r1 = (-b - sqrtDet) / (2f * a);
        float r2 = (-b + sqrtDet) / (2f * a);

        if (r1 > r2) {
            float temp = r2;
            r2 = r1;
            r1 = temp;
        }

        if (r1 > 0 && r1 < maxR) {
            return r1;
        }

        if (r2 > 0 && r2 < maxR) {
            return r2;
        }

        return Float.NaN;
    }

    private float collideWithVertex(Vector3f sCenter, Vector3f sVelocity,
                                    float velocitySquared, Vector3f vertex, float t) {
        // A = velocity * velocity
        // B = 2 * (velocity . (center - vertex));
        // C = ||vertex - center||^2 - 1f;

        temp1.set(sCenter).subtractLocal(vertex);
        float a = velocitySquared;
        float b = 2f * sVelocity.dot(temp1);
        float c = temp1.negateLocal().lengthSquared() - 1f;
        float newT = getLowestRoot(a, b, c, t);

//        float A = velocitySquared;
//        float B = sCenter.subtract(vertex).dot(sVelocity) * 2f;
//        float C = vertex.subtract(sCenter).lengthSquared() - 1f;
//
//        float newT = getLowestRoot(A, B, C, Float.MAX_VALUE);
//        if (newT > 1.0f)
//            newT = Float.NaN;

        return newT;
    }

    private float collideWithSegment(Vector3f sCenter,
                                     Vector3f sVelocity,
                                     float velocitySquared,
                                     Vector3f l1,
                                     Vector3f l2,
                                     float t,
                                     Vector3f store) {
        Vector3f edge = temp1.set(l2).subtractLocal(l1);
        Vector3f base = temp2.set(l1).subtractLocal(sCenter);

        float edgeSquared = edge.lengthSquared();
        float baseSquared = base.lengthSquared();

        float EdotV = edge.dot(sVelocity);
        float EdotB = edge.dot(base);

        float a = (edgeSquared * -velocitySquared) + EdotV * EdotV;
        float b = (edgeSquared * 2f * sVelocity.dot(base))
                - (2f * EdotV * EdotB);
        float c = (edgeSquared * (1f - baseSquared)) + EdotB * EdotB;
        float newT = getLowestRoot(a, b, c, t);
        if (!Float.isNaN(newT)) {
            float f = (EdotV * newT - EdotB) / edgeSquared;
            if (f >= 0f && f < 1f) {
                store.scaleAdd(f, edge, l1);
                return newT;
            }
        }
        return Float.NaN;
    }

    private CollisionResult collideWithTriangle(AbstractTriangle tri) {
        // scale scaledTriangle based on dimension
        scaledTri.get1().set(tri.get1()).multLocal(invDim);
        scaledTri.get2().set(tri.get2()).multLocal(invDim);
        scaledTri.get3().set(tri.get3()).multLocal(invDim);
//        Vector3f sVelocity = velocity.mult(invDim);
//        Vector3f sCenter = center.mult(invDim);
        velocity.mult(invDim, sVelocity);
        center.mult(invDim, sCenter);

        triPlane.setPlanePoints(scaledTri);

        float normalDotVelocity = triPlane.getNormal().dot(sVelocity);
        // XXX: sVelocity.normalize() needed?
        // back facing scaledTriangles not considered
        if (normalDotVelocity > 0f)
            return null;

        float t0, t1;
        boolean embedded = false;

        float signedDistanceToPlane = triPlane.pseudoDistance(sCenter);
        if (normalDotVelocity == 0.0f) {
            // we are travelling exactly parallel to the plane
            if (FastMath.abs(signedDistanceToPlane) >= 1.0f) {
                // no collision possible
                return null;
            } else {
                // we are embedded
                t0 = 0;
                t1 = 1;
                embedded = true;
                System.out.println("EMBEDDED");
                return null;
            }
        } else {
            t0 = (-1f - signedDistanceToPlane) / normalDotVelocity;
            t1 = (1f - signedDistanceToPlane) / normalDotVelocity;

            if (t0 > t1) {
                float tf = t1;
                t1 = t0;
                t0 = tf;
            }

            if (t0 > 1.0f || t1 < 0.0f) {
                // collision is out of this sVelocity range
                return null;
            }

            // clamp the interval to [0, 1]
            t0 = Math.max(t0, 0.0f);
            t1 = Math.min(t1, 1.0f);
        }

        boolean foundCollision = false;
        float minT = 1f;

        Vector3f contactPoint = new Vector3f();
        Vector3f contactNormal = new Vector3f();

//        if (!embedded){
        // check against the inside of the scaledTriangle
        // contactPoint = sCenter - p.normal + t0 * sVelocity
        contactPoint.set(sVelocity);
        contactPoint.multLocal(t0);
        contactPoint.addLocal(sCenter);
        contactPoint.subtractLocal(triPlane.getNormal());

        // test to see if the collision is on a scaledTriangle interior
        if (isPointInTriangle(contactPoint, scaledTri) && !embedded) {
            foundCollision = true;

            minT = t0;

            // scale collision point back into R3
            contactPoint.multLocal(dimension);
            contactNormal.set(velocity).multLocal(t0);
            contactNormal.addLocal(center);
            contactNormal.subtractLocal(contactPoint).normalizeLocal();

//                contactNormal.set(triPlane.getNormal());

            CollisionResult result = new CollisionResult();
            result.setContactPoint(contactPoint);
            result.setContactNormal(contactNormal);
            result.setDistance(minT * velocity.length());
            return result;
        }
//        }

        float velocitySquared = sVelocity.lengthSquared();

        Vector3f v1 = scaledTri.get1();
        Vector3f v2 = scaledTri.get2();
        Vector3f v3 = scaledTri.get3();

        // vertex 1
        float newT;
        newT = collideWithVertex(sCenter, sVelocity, velocitySquared, v1, minT);
        if (!Float.isNaN(newT)) {
            minT = newT;
            contactPoint.set(v1);
            foundCollision = true;
        }

        // vertex 2
        newT = collideWithVertex(sCenter, sVelocity, velocitySquared, v2, minT);
        if (!Float.isNaN(newT)) {
            minT = newT;
            contactPoint.set(v2);
            foundCollision = true;
        }

        // vertex 3
        newT = collideWithVertex(sCenter, sVelocity, velocitySquared, v3, minT);
        if (!Float.isNaN(newT)) {
            minT = newT;
            contactPoint.set(v3);
            foundCollision = true;
        }

        // edge 1-2
        newT = collideWithSegment(sCenter, sVelocity, velocitySquared, v1, v2, minT, contactPoint);
        if (!Float.isNaN(newT)) {
            minT = newT;
            foundCollision = true;
        }

        // edge 2-3
        newT = collideWithSegment(sCenter, sVelocity, velocitySquared, v2, v3, minT, contactPoint);
        if (!Float.isNaN(newT)) {
            minT = newT;
            foundCollision = true;
        }

        // edge 3-1
        newT = collideWithSegment(sCenter, sVelocity, velocitySquared, v3, v1, minT, contactPoint);
        if (!Float.isNaN(newT)) {
            minT = newT;
            foundCollision = true;
        }

        if (foundCollision) {
            // compute contact normal based on minimum t
            contactPoint.multLocal(dimension);
            contactNormal.set(velocity).multLocal(t0);
            contactNormal.addLocal(center);
            contactNormal.subtractLocal(contactPoint).normalizeLocal();

            CollisionResult result = new CollisionResult();
            result.setContactPoint(contactPoint);
            result.setContactNormal(contactNormal);
            result.setDistance(minT * velocity.length());

            return result;
        } else {
            return null;
        }
    }

    public CollisionResult collideWithSweepSphere(SweepSphere other) {
        temp1.set(velocity).subtractLocal(other.velocity);
        temp2.set(center).subtractLocal(other.center);
        temp3.set(dimension).addLocal(other.dimension);
        // delta V
        // delta C
        // Rsum

        float a = temp1.lengthSquared();
        float b = 2f * temp1.dot(temp2);
        float c = temp2.lengthSquared() - temp3.getX() * temp3.getX();
        float t = getLowestRoot(a, b, c, 1);

        // no collision
        if (Float.isNaN(t))
            return null;

        CollisionResult result = new CollisionResult();
        result.setDistance(velocity.length() * t);

        temp1.set(velocity).multLocal(t).addLocal(center);
        temp2.set(other.velocity).multLocal(t).addLocal(other.center);
        temp3.set(temp2).subtractLocal(temp1);
        // temp3 contains center to other.center vector

        // normalize it to get normal
        temp2.set(temp3).normalizeLocal();
        result.setContactNormal(new Vector3f(temp2));

        // temp3 is contact point
        temp3.set(temp2).multLocal(dimension).addLocal(temp1);
        result.setContactPoint(new Vector3f(temp3));

        return result;
    }

    public static void main(String[] args) {
        SweepSphere ss = new SweepSphere();
        ss.setCenter(Vector3f.ZERO);
        ss.setDimension(1);
        ss.setVelocity(new Vector3f(10, 10, 10));

        SweepSphere ss2 = new SweepSphere();
        ss2.setCenter(new Vector3f(5, 5, 5));
        ss2.setDimension(1);
        ss2.setVelocity(new Vector3f(-10, -10, -10));

        CollisionResults cr = new CollisionResults();
        ss.collideWith(ss2, cr);
        if (cr.size() > 0) {
            CollisionResult c = cr.getClosestCollision();
            System.out.println("D = " + c.getDistance());
            System.out.println("P = " + c.getContactPoint());
            System.out.println("N = " + c.getContactNormal());
        }
    }

    @Override
    public int collideWith(Collidable other, CollisionResults results)
            throws UnsupportedCollisionException {
        if (other instanceof AbstractTriangle) {
            AbstractTriangle tri = (AbstractTriangle) other;
            CollisionResult result = collideWithTriangle(tri);
            if (result != null) {
                results.addCollision(result);
                return 1;
            }
            return 0;
        } else if (other instanceof SweepSphere) {
            SweepSphere sph = (SweepSphere) other;

            CollisionResult result = collideWithSweepSphere(sph);
            if (result != null) {
                results.addCollision(result);
                return 1;
            }
            return 0;
        } else {
            throw new UnsupportedCollisionException();
        }
    }
}




© 2015 - 2024 Weber Informatics LLC | Privacy Policy