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.dynamics.joints; 25 26 import org.jbox2d.common.MathUtils; 27 import org.jbox2d.common.Settings; 28 import org.jbox2d.common.Vec2; 29 import org.jbox2d.dynamics.Body; 30 import org.jbox2d.dynamics.SolverData; 31 import org.jbox2d.dynamics.World; 32 import org.jbox2d.dynamics.contacts.Position; 33 import org.jbox2d.dynamics.contacts.Velocity; 34 35 public class ConstantVolumeJoint extends Joint { 36 37 private final Body[] bodies; 38 private float[] targetLengths; 39 private float targetVolume; 40 41 private Vec2[] normals; 42 private float m_impulse = 0.0f; 43 44 private World world; 45 46 private DistanceJoint[] distanceJoints; 47 48 public Body[] getBodies() { 49 return bodies; 50 } 51 52 public DistanceJoint[] getJoints() { 53 return distanceJoints; 54 } 55 56 public void inflate(float factor) { 57 targetVolume *= factor; 58 } 59 60 public ConstantVolumeJoint(World argWorld, ConstantVolumeJointDef def) { 61 super(argWorld.getPool(), def); 62 world = argWorld; 63 if (def.bodies.size() <= 2) { 64 throw new IllegalArgumentException( 65 "You cannot create a constant volume joint with less than three bodies."); 66 } 67 bodies = def.bodies.toArray(new Body[0]); 68 69 targetLengths = new float[bodies.length]; 70 for (int i = 0; i < targetLengths.length; ++i) { 71 final int next = (i == targetLengths.length - 1) ? 0 : i + 1; 72 float dist = bodies[i].getWorldCenter().sub(bodies[next].getWorldCenter()).length(); 73 targetLengths[i] = dist; 74 } 75 targetVolume = getBodyArea(); 76 77 if (def.joints != null && def.joints.size() != def.bodies.size()) { 78 throw new IllegalArgumentException( 79 "Incorrect joint definition. Joints have to correspond to the bodies"); 80 } 81 if (def.joints == null) { 82 final DistanceJointDef djd = new DistanceJointDef(); 83 distanceJoints = new DistanceJoint[bodies.length]; 84 for (int i = 0; i < targetLengths.length; ++i) { 85 final int next = (i == targetLengths.length - 1) ? 0 : i + 1; 86 djd.frequencyHz = def.frequencyHz;// 20.0f; 87 djd.dampingRatio = def.dampingRatio;// 50.0f; 88 djd.collideConnected = def.collideConnected; 89 djd.initialize(bodies[i], bodies[next], bodies[i].getWorldCenter(), 90 bodies[next].getWorldCenter()); 91 distanceJoints[i] = (DistanceJoint) world.createJoint(djd); 92 } 93 } else { 94 distanceJoints = def.joints.toArray(new DistanceJoint[0]); 95 } 96 97 normals = new Vec2[bodies.length]; 98 for (int i = 0; i < normals.length; ++i) { 99 normals[i] = new Vec2(); 100 } 101 } 102 103 @Override 104 public void destructor() { 105 for (int i = 0; i < distanceJoints.length; ++i) { 106 world.destroyJoint(distanceJoints[i]); 107 } 108 } 109 110 private float getBodyArea() { 111 float area = 0.0f; 112 for (int i = 0; i < bodies.length; ++i) { 113 final int next = (i == bodies.length - 1) ? 0 : i + 1; 114 area += 115 bodies[i].getWorldCenter().x * bodies[next].getWorldCenter().y 116 - bodies[next].getWorldCenter().x * bodies[i].getWorldCenter().y; 117 } 118 area *= .5f; 119 return area; 120 } 121 122 private float getSolverArea(Position[] positions) { 123 float area = 0.0f; 124 for (int i = 0; i < bodies.length; ++i) { 125 final int next = (i == bodies.length - 1) ? 0 : i + 1; 126 area += 127 positions[bodies[i].m_islandIndex].c.x * positions[bodies[next].m_islandIndex].c.y 128 - positions[bodies[next].m_islandIndex].c.x * positions[bodies[i].m_islandIndex].c.y; 129 } 130 area *= .5f; 131 return area; 132 } 133 134 private boolean constrainEdges(Position[] positions) { 135 float perimeter = 0.0f; 136 for (int i = 0; i < bodies.length; ++i) { 137 final int next = (i == bodies.length - 1) ? 0 : i + 1; 138 float dx = positions[bodies[next].m_islandIndex].c.x - positions[bodies[i].m_islandIndex].c.x; 139 float dy = positions[bodies[next].m_islandIndex].c.y - positions[bodies[i].m_islandIndex].c.y; 140 float dist = MathUtils.sqrt(dx * dx + dy * dy); 141 if (dist < Settings.EPSILON) { 142 dist = 1.0f; 143 } 144 normals[i].x = dy / dist; 145 normals[i].y = -dx / dist; 146 perimeter += dist; 147 } 148 149 final Vec2 delta = pool.popVec2(); 150 151 float deltaArea = targetVolume - getSolverArea(positions); 152 float toExtrude = 0.5f * deltaArea / perimeter; // *relaxationFactor 153 // float sumdeltax = 0.0f; 154 boolean done = true; 155 for (int i = 0; i < bodies.length; ++i) { 156 final int next = (i == bodies.length - 1) ? 0 : i + 1; 157 delta.set(toExtrude * (normals[i].x + normals[next].x), toExtrude 158 * (normals[i].y + normals[next].y)); 159 // sumdeltax += dx; 160 float normSqrd = delta.lengthSquared(); 161 if (normSqrd > Settings.maxLinearCorrection * Settings.maxLinearCorrection) { 162 delta.mulLocal(Settings.maxLinearCorrection / MathUtils.sqrt(normSqrd)); 163 } 164 if (normSqrd > Settings.linearSlop * Settings.linearSlop) { 165 done = false; 166 } 167 positions[bodies[next].m_islandIndex].c.x += delta.x; 168 positions[bodies[next].m_islandIndex].c.y += delta.y; 169 // bodies[next].m_linearVelocity.x += delta.x * step.inv_dt; 170 // bodies[next].m_linearVelocity.y += delta.y * step.inv_dt; 171 } 172 173 pool.pushVec2(1); 174 // System.out.println(sumdeltax); 175 return done; 176 } 177 178 @Override 179 public void initVelocityConstraints(final SolverData step) { 180 Velocity[] velocities = step.velocities; 181 Position[] positions = step.positions; 182 final Vec2[] d = pool.getVec2Array(bodies.length); 183 184 for (int i = 0; i < bodies.length; ++i) { 185 final int prev = (i == 0) ? bodies.length - 1 : i - 1; 186 final int next = (i == bodies.length - 1) ? 0 : i + 1; 187 d[i].set(positions[bodies[next].m_islandIndex].c); 188 d[i].subLocal(positions[bodies[prev].m_islandIndex].c); 189 } 190 191 if (step.step.warmStarting) { 192 m_impulse *= step.step.dtRatio; 193 // float lambda = -2.0f * crossMassSum / dotMassSum; 194 // System.out.println(crossMassSum + " " +dotMassSum); 195 // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, 196 // Settings.maxLinearCorrection); 197 // m_impulse = lambda; 198 for (int i = 0; i < bodies.length; ++i) { 199 velocities[bodies[i].m_islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * m_impulse; 200 velocities[bodies[i].m_islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * m_impulse; 201 } 202 } else { 203 m_impulse = 0.0f; 204 } 205 } 206 207 @Override 208 public boolean solvePositionConstraints(SolverData step) { 209 return constrainEdges(step.positions); 210 } 211 212 @Override 213 public void solveVelocityConstraints(final SolverData step) { 214 float crossMassSum = 0.0f; 215 float dotMassSum = 0.0f; 216 217 Velocity[] velocities = step.velocities; 218 Position[] positions = step.positions; 219 final Vec2 d[] = pool.getVec2Array(bodies.length); 220 221 for (int i = 0; i < bodies.length; ++i) { 222 final int prev = (i == 0) ? bodies.length - 1 : i - 1; 223 final int next = (i == bodies.length - 1) ? 0 : i + 1; 224 d[i].set(positions[bodies[next].m_islandIndex].c); 225 d[i].subLocal(positions[bodies[prev].m_islandIndex].c); 226 dotMassSum += (d[i].lengthSquared()) / bodies[i].getMass(); 227 crossMassSum += Vec2.cross(velocities[bodies[i].m_islandIndex].v, d[i]); 228 } 229 float lambda = -2.0f * crossMassSum / dotMassSum; 230 // System.out.println(crossMassSum + " " +dotMassSum); 231 // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, 232 // Settings.maxLinearCorrection); 233 m_impulse += lambda; 234 // System.out.println(m_impulse); 235 for (int i = 0; i < bodies.length; ++i) { 236 velocities[bodies[i].m_islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * lambda; 237 velocities[bodies[i].m_islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * lambda; 238 } 239 } 240 241 /** No-op */ 242 @Override 243 public void getAnchorA(Vec2 argOut) {} 244 245 /** No-op */ 246 @Override 247 public void getAnchorB(Vec2 argOut) {} 248 249 /** No-op */ 250 @Override 251 public void getReactionForce(float inv_dt, Vec2 argOut) {} 252 253 /** No-op */ 254 @Override 255 public float getReactionTorque(float inv_dt) { 256 return 0; 257 } 258 } 259