diff options
Diffstat (limited to 'engine/src/core/com/jme3/collision/SweepSphere.java')
| -rw-r--r-- | engine/src/core/com/jme3/collision/SweepSphere.java | 440 |
1 files changed, 440 insertions, 0 deletions
diff --git a/engine/src/core/com/jme3/collision/SweepSphere.java b/engine/src/core/com/jme3/collision/SweepSphere.java new file mode 100644 index 0000000..9d43f89 --- /dev/null +++ b/engine/src/core/com/jme3/collision/SweepSphere.java @@ -0,0 +1,440 @@ +/* + * Copyright (c) 2009-2010 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 { + + private Vector3f velocity = new Vector3f(); + private Vector3f center = new Vector3f(); + private Vector3f dimension = new Vector3f(); + 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 sqrtd = FastMath.sqrt(determinant); + float r1 = (-b - sqrtd) / (2f * a); + float r2 = (-b + sqrtd) / (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 parrallel 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()); + } + } + + 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(); + } + } + +} |