1 /******************************************************************************* 2 * Copyright (c) 2013, Daniel Murphy 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without modification, 6 * are permitted provided that the following conditions are met: 7 * * Redistributions of source code must retain the above copyright notice, 8 * this list of conditions and the following disclaimer. 9 * * Redistributions in binary form must reproduce the above copyright notice, 10 * this list of conditions and the following disclaimer in the documentation 11 * and/or other materials provided with the distribution. 12 * 13 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 16 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 17 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 18 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 19 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 20 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 21 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 22 * POSSIBILITY OF SUCH DAMAGE. 23 ******************************************************************************/ 24 package org.jbox2d.collision; 25 26 import org.jbox2d.common.MathUtils; 27 import org.jbox2d.common.Rot; 28 import org.jbox2d.common.Settings; 29 import org.jbox2d.common.Transform; 30 import org.jbox2d.common.Vec2; 31 32 /** 33 * This is used to compute the current state of a contact manifold. 34 * 35 * @author daniel 36 */ 37 public class WorldManifold { 38 /** 39 * World vector pointing from A to B 40 */ 41 public final Vec2 normal; 42 43 /** 44 * World contact point (point of intersection) 45 */ 46 public final Vec2[] points; 47 48 /** 49 * A negative value indicates overlap, in meters. 50 */ 51 public final float[] separations; 52 53 public WorldManifold() { 54 normal = new Vec2(); 55 points = new Vec2[Settings.maxManifoldPoints]; 56 separations = new float[Settings.maxManifoldPoints]; 57 for (int i = 0; i < Settings.maxManifoldPoints; i++) { 58 points[i] = new Vec2(); 59 } 60 } 61 62 private final Vec2 pool3 = new Vec2(); 63 private final Vec2 pool4 = new Vec2(); 64 65 public final void initialize(final Manifold manifold, final Transform xfA, float radiusA, 66 final Transform xfB, float radiusB) { 67 if (manifold.pointCount == 0) { 68 return; 69 } 70 71 switch (manifold.type) { 72 case CIRCLES: { 73 final Vec2 pointA = pool3; 74 final Vec2 pointB = pool4; 75 76 normal.x = 1; 77 normal.y = 0; 78 Vec2 v = manifold.localPoint; 79 // Transform.mulToOutUnsafe(xfA, manifold.localPoint, pointA); 80 // Transform.mulToOutUnsafe(xfB, manifold.points[0].localPoint, pointB); 81 pointA.x = (xfA.q.c * v.x - xfA.q.s * v.y) + xfA.p.x; 82 pointA.y = (xfA.q.s * v.x + xfA.q.c * v.y) + xfA.p.y; 83 Vec2 mp0p = manifold.points[0].localPoint; 84 pointB.x = (xfB.q.c * mp0p.x - xfB.q.s * mp0p.y) + xfB.p.x; 85 pointB.y = (xfB.q.s * mp0p.x + xfB.q.c * mp0p.y) + xfB.p.y; 86 87 if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { 88 normal.x = pointB.x - pointA.x; 89 normal.y = pointB.y - pointA.y; 90 normal.normalize(); 91 } 92 93 final float cAx = normal.x * radiusA + pointA.x; 94 final float cAy = normal.y * radiusA + pointA.y; 95 96 final float cBx = -normal.x * radiusB + pointB.x; 97 final float cBy = -normal.y * radiusB + pointB.y; 98 99 points[0].x = (cAx + cBx) * .5f; 100 points[0].y = (cAy + cBy) * .5f; 101 separations[0] = (cBx - cAx) * normal.x + (cBy - cAy) * normal.y; 102 } 103 break; 104 case FACE_A: { 105 final Vec2 planePoint = pool3; 106 107 Rot.mulToOutUnsafe(xfA.q, manifold.localNormal, normal); 108 Transform.mulToOut(xfA, manifold.localPoint, planePoint); 109 110 final Vec2 clipPoint = pool4; 111 112 for (int i = 0; i < manifold.pointCount; i++) { 113 // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); 114 // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, 115 // normal)) * normal; 116 // b2Vec2 cB = clipPoint - radiusB * normal; 117 // points[i] = 0.5f * (cA + cB); 118 Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint); 119 // use cA as temporary for now 120 // cA.set(clipPoint).subLocal(planePoint); 121 // float scalar = radiusA - Vec2.dot(cA, normal); 122 // cA.set(normal).mulLocal(scalar).addLocal(clipPoint); 123 // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal(); 124 // points[i].set(cA).addLocal(cB).mulLocal(0.5f); 125 126 final float scalar = 127 radiusA 128 - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) 129 * normal.y); 130 131 final float cAx = normal.x * scalar + clipPoint.x; 132 final float cAy = normal.y * scalar + clipPoint.y; 133 134 final float cBx = -normal.x * radiusB + clipPoint.x; 135 final float cBy = -normal.y * radiusB + clipPoint.y; 136 137 points[i].x = (cAx + cBx) * .5f; 138 points[i].y = (cAy + cBy) * .5f; 139 separations[i] = (cBx - cAx) * normal.x + (cBy - cAy) * normal.y; 140 } 141 } 142 break; 143 case FACE_B: 144 final Vec2 planePoint = pool3; 145 Rot.mulToOutUnsafe(xfB.q, manifold.localNormal, normal); 146 Transform.mulToOut(xfB, manifold.localPoint, planePoint); 147 148 // final Mat22 R = xfB.q; 149 // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y; 150 // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y; 151 // final Vec2 v = manifold.localPoint; 152 // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y; 153 // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y; 154 155 final Vec2 clipPoint = pool4; 156 157 for (int i = 0; i < manifold.pointCount; i++) { 158 // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); 159 // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, 160 // normal)) * normal; 161 // b2Vec2 cA = clipPoint - radiusA * normal; 162 // points[i] = 0.5f * (cA + cB); 163 164 Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint); 165 // cB.set(clipPoint).subLocal(planePoint); 166 // float scalar = radiusB - Vec2.dot(cB, normal); 167 // cB.set(normal).mulLocal(scalar).addLocal(clipPoint); 168 // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal(); 169 // points[i].set(cA).addLocal(cB).mulLocal(0.5f); 170 171 // points[i] = 0.5f * (cA + cB); 172 173 // 174 // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x * 175 // manifold.points[i].localPoint.y; 176 // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y * 177 // manifold.points[i].localPoint.y; 178 179 final float scalar = 180 radiusB 181 - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) 182 * normal.y); 183 184 final float cBx = normal.x * scalar + clipPoint.x; 185 final float cBy = normal.y * scalar + clipPoint.y; 186 187 final float cAx = -normal.x * radiusA + clipPoint.x; 188 final float cAy = -normal.y * radiusA + clipPoint.y; 189 190 points[i].x = (cAx + cBx) * .5f; 191 points[i].y = (cAy + cBy) * .5f; 192 separations[i] = (cAx - cBx) * normal.x + (cAy - cBy) * normal.y; 193 } 194 // Ensure normal points from A to B. 195 normal.x = -normal.x; 196 normal.y = -normal.y; 197 break; 198 } 199 } 200 } 201
