1 /* 2 * Copyright (c) 2009-2010 jMonkeyEngine 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are 7 * met: 8 * 9 * * Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 12 * * Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 22 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 27 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 28 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 29 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 */ 32 33 package com.jme3.collision; 34 35 import com.jme3.math.*; 36 37 /** 38 * No longer public .. 39 * 40 * @author Kirill Vainer 41 */ 42 @Deprecated 43 class SweepSphere implements Collidable { 44 45 private Vector3f velocity = new Vector3f(); 46 private Vector3f center = new Vector3f(); 47 private Vector3f dimension = new Vector3f(); 48 private Vector3f invDim = new Vector3f(); 49 50 private final Triangle scaledTri = new Triangle(); 51 private final Plane triPlane = new Plane(); 52 private final Vector3f temp1 = new Vector3f(), 53 temp2 = new Vector3f(), 54 temp3 = new Vector3f(); 55 private final Vector3f sVelocity = new Vector3f(), 56 sCenter = new Vector3f(); 57 58 public Vector3f getCenter() { 59 return center; 60 } 61 62 public void setCenter(Vector3f center) { 63 this.center.set(center); 64 } 65 66 public Vector3f getDimension() { 67 return dimension; 68 } 69 70 public void setDimension(Vector3f dimension) { 71 this.dimension.set(dimension); 72 this.invDim.set(1,1,1).divideLocal(dimension); 73 } 74 75 public void setDimension(float x, float y, float z){ 76 this.dimension.set(x,y,z); 77 this.invDim.set(1,1,1).divideLocal(dimension); 78 } 79 80 public void setDimension(float dim){ 81 this.dimension.set(dim, dim, dim); 82 this.invDim.set(1,1,1).divideLocal(dimension); 83 } 84 85 public Vector3f getVelocity() { 86 return velocity; 87 } 88 89 public void setVelocity(Vector3f velocity) { 90 this.velocity.set(velocity); 91 } 92 93 private boolean pointsOnSameSide(Vector3f p1, Vector3f p2, Vector3f line1, Vector3f line2) { 94 // V1 = (line2 - line1) x (p1 - line1) 95 // V2 = (p2 - line1) x (line2 - line1) 96 97 temp1.set(line2).subtractLocal(line1); 98 temp3.set(temp1); 99 temp2.set(p1).subtractLocal(line1); 100 temp1.crossLocal(temp2); 101 102 temp2.set(p2).subtractLocal(line1); 103 temp3.crossLocal(temp2); 104 105 // V1 . V2 >= 0 106 return temp1.dot(temp3) >= 0; 107 } 108 109 private boolean isPointInTriangle(Vector3f point, AbstractTriangle tri) { 110 if (pointsOnSameSide(point, tri.get1(), tri.get2(), tri.get3()) 111 && pointsOnSameSide(point, tri.get2(), tri.get1(), tri.get3()) 112 && pointsOnSameSide(point, tri.get3(), tri.get1(), tri.get2())) 113 return true; 114 return false; 115 } 116 117 private static float getLowestRoot(float a, float b, float c, float maxR) { 118 float determinant = b * b - 4f * a * c; 119 if (determinant < 0){ 120 return Float.NaN; 121 } 122 123 float sqrtd = FastMath.sqrt(determinant); 124 float r1 = (-b - sqrtd) / (2f * a); 125 float r2 = (-b + sqrtd) / (2f * a); 126 127 if (r1 > r2){ 128 float temp = r2; 129 r2 = r1; 130 r1 = temp; 131 } 132 133 if (r1 > 0 && r1 < maxR){ 134 return r1; 135 } 136 137 if (r2 > 0 && r2 < maxR){ 138 return r2; 139 } 140 141 return Float.NaN; 142 } 143 144 private float collideWithVertex(Vector3f sCenter, Vector3f sVelocity, 145 float velocitySquared, Vector3f vertex, float t) { 146 // A = velocity * velocity 147 // B = 2 * (velocity . (center - vertex)); 148 // C = ||vertex - center||^2 - 1f; 149 150 temp1.set(sCenter).subtractLocal(vertex); 151 float a = velocitySquared; 152 float b = 2f * sVelocity.dot(temp1); 153 float c = temp1.negateLocal().lengthSquared() - 1f; 154 float newT = getLowestRoot(a, b, c, t); 155 156 // float A = velocitySquared; 157 // float B = sCenter.subtract(vertex).dot(sVelocity) * 2f; 158 // float C = vertex.subtract(sCenter).lengthSquared() - 1f; 159 // 160 // float newT = getLowestRoot(A, B, C, Float.MAX_VALUE); 161 // if (newT > 1.0f) 162 // newT = Float.NaN; 163 164 return newT; 165 } 166 167 private float collideWithSegment(Vector3f sCenter, 168 Vector3f sVelocity, 169 float velocitySquared, 170 Vector3f l1, 171 Vector3f l2, 172 float t, 173 Vector3f store) { 174 Vector3f edge = temp1.set(l2).subtractLocal(l1); 175 Vector3f base = temp2.set(l1).subtractLocal(sCenter); 176 177 float edgeSquared = edge.lengthSquared(); 178 float baseSquared = base.lengthSquared(); 179 180 float EdotV = edge.dot(sVelocity); 181 float EdotB = edge.dot(base); 182 183 float a = (edgeSquared * -velocitySquared) + EdotV * EdotV; 184 float b = (edgeSquared * 2f * sVelocity.dot(base)) 185 - (2f * EdotV * EdotB); 186 float c = (edgeSquared * (1f - baseSquared)) + EdotB * EdotB; 187 float newT = getLowestRoot(a, b, c, t); 188 if (!Float.isNaN(newT)){ 189 float f = (EdotV * newT - EdotB) / edgeSquared; 190 if (f >= 0f && f < 1f){ 191 store.scaleAdd(f, edge, l1); 192 return newT; 193 } 194 } 195 return Float.NaN; 196 } 197 198 private CollisionResult collideWithTriangle(AbstractTriangle tri){ 199 // scale scaledTriangle based on dimension 200 scaledTri.get1().set(tri.get1()).multLocal(invDim); 201 scaledTri.get2().set(tri.get2()).multLocal(invDim); 202 scaledTri.get3().set(tri.get3()).multLocal(invDim); 203 // Vector3f sVelocity = velocity.mult(invDim); 204 // Vector3f sCenter = center.mult(invDim); 205 velocity.mult(invDim, sVelocity); 206 center.mult(invDim, sCenter); 207 208 triPlane.setPlanePoints(scaledTri); 209 210 float normalDotVelocity = triPlane.getNormal().dot(sVelocity); 211 // XXX: sVelocity.normalize() needed? 212 // back facing scaledTriangles not considered 213 if (normalDotVelocity > 0f) 214 return null; 215 216 float t0, t1; 217 boolean embedded = false; 218 219 float signedDistanceToPlane = triPlane.pseudoDistance(sCenter); 220 if (normalDotVelocity == 0.0f){ 221 // we are travelling exactly parrallel to the plane 222 if (FastMath.abs(signedDistanceToPlane) >= 1.0f){ 223 // no collision possible 224 return null; 225 }else{ 226 // we are embedded 227 t0 = 0; 228 t1 = 1; 229 embedded = true; 230 System.out.println("EMBEDDED"); 231 return null; 232 } 233 }else{ 234 t0 = (-1f - signedDistanceToPlane) / normalDotVelocity; 235 t1 = ( 1f - signedDistanceToPlane) / normalDotVelocity; 236 237 if (t0 > t1){ 238 float tf = t1; 239 t1 = t0; 240 t0 = tf; 241 } 242 243 if (t0 > 1.0f || t1 < 0.0f){ 244 // collision is out of this sVelocity range 245 return null; 246 } 247 248 // clamp the interval to [0, 1] 249 t0 = Math.max(t0, 0.0f); 250 t1 = Math.min(t1, 1.0f); 251 } 252 253 boolean foundCollision = false; 254 float minT = 1f; 255 256 Vector3f contactPoint = new Vector3f(); 257 Vector3f contactNormal = new Vector3f(); 258 259 // if (!embedded){ 260 // check against the inside of the scaledTriangle 261 // contactPoint = sCenter - p.normal + t0 * sVelocity 262 contactPoint.set(sVelocity); 263 contactPoint.multLocal(t0); 264 contactPoint.addLocal(sCenter); 265 contactPoint.subtractLocal(triPlane.getNormal()); 266 267 // test to see if the collision is on a scaledTriangle interior 268 if (isPointInTriangle(contactPoint, scaledTri) && !embedded){ 269 foundCollision = true; 270 271 minT = t0; 272 273 // scale collision point back into R3 274 contactPoint.multLocal(dimension); 275 contactNormal.set(velocity).multLocal(t0); 276 contactNormal.addLocal(center); 277 contactNormal.subtractLocal(contactPoint).normalizeLocal(); 278 279 // contactNormal.set(triPlane.getNormal()); 280 281 CollisionResult result = new CollisionResult(); 282 result.setContactPoint(contactPoint); 283 result.setContactNormal(contactNormal); 284 result.setDistance(minT * velocity.length()); 285 return result; 286 } 287 // } 288 289 float velocitySquared = sVelocity.lengthSquared(); 290 291 Vector3f v1 = scaledTri.get1(); 292 Vector3f v2 = scaledTri.get2(); 293 Vector3f v3 = scaledTri.get3(); 294 295 // vertex 1 296 float newT; 297 newT = collideWithVertex(sCenter, sVelocity, velocitySquared, v1, minT); 298 if (!Float.isNaN(newT)){ 299 minT = newT; 300 contactPoint.set(v1); 301 foundCollision = true; 302 } 303 304 // vertex 2 305 newT = collideWithVertex(sCenter, sVelocity, velocitySquared, v2, minT); 306 if (!Float.isNaN(newT)){ 307 minT = newT; 308 contactPoint.set(v2); 309 foundCollision = true; 310 } 311 312 // vertex 3 313 newT = collideWithVertex(sCenter, sVelocity, velocitySquared, v3, minT); 314 if (!Float.isNaN(newT)){ 315 minT = newT; 316 contactPoint.set(v3); 317 foundCollision = true; 318 } 319 320 // edge 1-2 321 newT = collideWithSegment(sCenter, sVelocity, velocitySquared, v1, v2, minT, contactPoint); 322 if (!Float.isNaN(newT)){ 323 minT = newT; 324 foundCollision = true; 325 } 326 327 // edge 2-3 328 newT = collideWithSegment(sCenter, sVelocity, velocitySquared, v2, v3, minT, contactPoint); 329 if (!Float.isNaN(newT)){ 330 minT = newT; 331 foundCollision = true; 332 } 333 334 // edge 3-1 335 newT = collideWithSegment(sCenter, sVelocity, velocitySquared, v3, v1, minT, contactPoint); 336 if (!Float.isNaN(newT)){ 337 minT = newT; 338 foundCollision = true; 339 } 340 341 if (foundCollision){ 342 // compute contact normal based on minimum t 343 contactPoint.multLocal(dimension); 344 contactNormal.set(velocity).multLocal(t0); 345 contactNormal.addLocal(center); 346 contactNormal.subtractLocal(contactPoint).normalizeLocal(); 347 348 CollisionResult result = new CollisionResult(); 349 result.setContactPoint(contactPoint); 350 result.setContactNormal(contactNormal); 351 result.setDistance(minT * velocity.length()); 352 353 return result; 354 }else{ 355 return null; 356 } 357 } 358 359 public CollisionResult collideWithSweepSphere(SweepSphere other){ 360 temp1.set(velocity).subtractLocal(other.velocity); 361 temp2.set(center).subtractLocal(other.center); 362 temp3.set(dimension).addLocal(other.dimension); 363 // delta V 364 // delta C 365 // Rsum 366 367 float a = temp1.lengthSquared(); 368 float b = 2f * temp1.dot(temp2); 369 float c = temp2.lengthSquared() - temp3.getX() * temp3.getX(); 370 float t = getLowestRoot(a, b, c, 1); 371 372 // no collision 373 if (Float.isNaN(t)) 374 return null; 375 376 CollisionResult result = new CollisionResult(); 377 result.setDistance(velocity.length() * t); 378 379 temp1.set(velocity).multLocal(t).addLocal(center); 380 temp2.set(other.velocity).multLocal(t).addLocal(other.center); 381 temp3.set(temp2).subtractLocal(temp1); 382 // temp3 contains center to other.center vector 383 384 // normalize it to get normal 385 temp2.set(temp3).normalizeLocal(); 386 result.setContactNormal(new Vector3f(temp2)); 387 388 // temp3 is contact point 389 temp3.set(temp2).multLocal(dimension).addLocal(temp1); 390 result.setContactPoint(new Vector3f(temp3)); 391 392 return result; 393 } 394 395 public static void main(String[] args){ 396 SweepSphere ss = new SweepSphere(); 397 ss.setCenter(Vector3f.ZERO); 398 ss.setDimension(1); 399 ss.setVelocity(new Vector3f(10, 10, 10)); 400 401 SweepSphere ss2 = new SweepSphere(); 402 ss2.setCenter(new Vector3f(5, 5, 5)); 403 ss2.setDimension(1); 404 ss2.setVelocity(new Vector3f(-10, -10, -10)); 405 406 CollisionResults cr = new CollisionResults(); 407 ss.collideWith(ss2, cr); 408 if (cr.size() > 0){ 409 CollisionResult c = cr.getClosestCollision(); 410 System.out.println("D = "+c.getDistance()); 411 System.out.println("P = "+c.getContactPoint()); 412 System.out.println("N = "+c.getContactNormal()); 413 } 414 } 415 416 public int collideWith(Collidable other, CollisionResults results) 417 throws UnsupportedCollisionException { 418 if (other instanceof AbstractTriangle){ 419 AbstractTriangle tri = (AbstractTriangle) other; 420 CollisionResult result = collideWithTriangle(tri); 421 if (result != null){ 422 results.addCollision(result); 423 return 1; 424 } 425 return 0; 426 }else if (other instanceof SweepSphere){ 427 SweepSphere sph = (SweepSphere) other; 428 429 CollisionResult result = collideWithSweepSphere(sph); 430 if (result != null){ 431 results.addCollision(result); 432 return 1; 433 } 434 return 0; 435 }else{ 436 throw new UnsupportedCollisionException(); 437 } 438 } 439 440 } 441