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.Rot; 28 import org.jbox2d.common.Settings; 29 import org.jbox2d.common.Vec2; 30 import org.jbox2d.dynamics.Body; 31 import org.jbox2d.dynamics.SolverData; 32 import org.jbox2d.pooling.IWorldPool; 33 34 //Linear constraint (point-to-line) 35 //d = pB - pA = xB + rB - xA - rA 36 //C = dot(ay, d) 37 //Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) 38 // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) 39 //J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] 40 41 //Spring linear constraint 42 //C = dot(ax, d) 43 //Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) 44 //J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] 45 46 //Motor rotational constraint 47 //Cdot = wB - wA 48 //J = [0 0 -1 0 0 1] 49 50 /** 51 * A wheel joint. This joint provides two degrees of freedom: translation along an axis fixed in 52 * bodyA and rotation in the plane. You can use a joint limit to restrict the range of motion and a 53 * joint motor to drive the rotation or to model rotational friction. This joint is designed for 54 * vehicle suspensions. 55 * 56 * @author Daniel Murphy 57 */ 58 public class WheelJoint extends Joint { 59 60 private float m_frequencyHz; 61 private float m_dampingRatio; 62 63 // Solver shared 64 private final Vec2 m_localAnchorA = new Vec2(); 65 private final Vec2 m_localAnchorB = new Vec2(); 66 private final Vec2 m_localXAxisA = new Vec2(); 67 private final Vec2 m_localYAxisA = new Vec2(); 68 69 private float m_impulse; 70 private float m_motorImpulse; 71 private float m_springImpulse; 72 73 private float m_maxMotorTorque; 74 private float m_motorSpeed; 75 private boolean m_enableMotor; 76 77 // Solver temp 78 private int m_indexA; 79 private int m_indexB; 80 private final Vec2 m_localCenterA = new Vec2(); 81 private final Vec2 m_localCenterB = new Vec2(); 82 private float m_invMassA; 83 private float m_invMassB; 84 private float m_invIA; 85 private float m_invIB; 86 87 private final Vec2 m_ax = new Vec2(); 88 private final Vec2 m_ay = new Vec2(); 89 private float m_sAx, m_sBx; 90 private float m_sAy, m_sBy; 91 92 private float m_mass; 93 private float m_motorMass; 94 private float m_springMass; 95 96 private float m_bias; 97 private float m_gamma; 98 99 protected WheelJoint(IWorldPool argPool, WheelJointDef def) { 100 super(argPool, def); 101 m_localAnchorA.set(def.localAnchorA); 102 m_localAnchorB.set(def.localAnchorB); 103 m_localXAxisA.set(def.localAxisA); 104 Vec2.crossToOutUnsafe(1.0f, m_localXAxisA, m_localYAxisA); 105 106 107 m_motorMass = 0.0f; 108 m_motorImpulse = 0.0f; 109 110 m_maxMotorTorque = def.maxMotorTorque; 111 m_motorSpeed = def.motorSpeed; 112 m_enableMotor = def.enableMotor; 113 114 m_frequencyHz = def.frequencyHz; 115 m_dampingRatio = def.dampingRatio; 116 } 117 118 public Vec2 getLocalAnchorA() { 119 return m_localAnchorA; 120 } 121 122 public Vec2 getLocalAnchorB() { 123 return m_localAnchorB; 124 } 125 126 @Override 127 public void getAnchorA(Vec2 argOut) { 128 m_bodyA.getWorldPointToOut(m_localAnchorA, argOut); 129 } 130 131 @Override 132 public void getAnchorB(Vec2 argOut) { 133 m_bodyB.getWorldPointToOut(m_localAnchorB, argOut); 134 } 135 136 @Override 137 public void getReactionForce(float inv_dt, Vec2 argOut) { 138 final Vec2 temp = pool.popVec2(); 139 temp.set(m_ay).mulLocal(m_impulse); 140 argOut.set(m_ax).mulLocal(m_springImpulse).addLocal(temp).mulLocal(inv_dt); 141 pool.pushVec2(1); 142 } 143 144 @Override 145 public float getReactionTorque(float inv_dt) { 146 return inv_dt * m_motorImpulse; 147 } 148 149 public float getJointTranslation() { 150 Body b1 = m_bodyA; 151 Body b2 = m_bodyB; 152 153 Vec2 p1 = pool.popVec2(); 154 Vec2 p2 = pool.popVec2(); 155 Vec2 axis = pool.popVec2(); 156 b1.getWorldPointToOut(m_localAnchorA, p1); 157 b2.getWorldPointToOut(m_localAnchorA, p2); 158 p2.subLocal(p1); 159 b1.getWorldVectorToOut(m_localXAxisA, axis); 160 161 float translation = Vec2.dot(p2, axis); 162 pool.pushVec2(3); 163 return translation; 164 } 165 166 /** For serialization */ 167 public Vec2 getLocalAxisA() { 168 return m_localXAxisA; 169 } 170 171 public float getJointSpeed() { 172 return m_bodyA.m_angularVelocity - m_bodyB.m_angularVelocity; 173 } 174 175 public boolean isMotorEnabled() { 176 return m_enableMotor; 177 } 178 179 public void enableMotor(boolean flag) { 180 m_bodyA.setAwake(true); 181 m_bodyB.setAwake(true); 182 m_enableMotor = flag; 183 } 184 185 public void setMotorSpeed(float speed) { 186 m_bodyA.setAwake(true); 187 m_bodyB.setAwake(true); 188 m_motorSpeed = speed; 189 } 190 191 public float getMotorSpeed() { 192 return m_motorSpeed; 193 } 194 195 public float getMaxMotorTorque() { 196 return m_maxMotorTorque; 197 } 198 199 public void setMaxMotorTorque(float torque) { 200 m_bodyA.setAwake(true); 201 m_bodyB.setAwake(true); 202 m_maxMotorTorque = torque; 203 } 204 205 public float getMotorTorque(float inv_dt) { 206 return m_motorImpulse * inv_dt; 207 } 208 209 public void setSpringFrequencyHz(float hz) { 210 m_frequencyHz = hz; 211 } 212 213 public float getSpringFrequencyHz() { 214 return m_frequencyHz; 215 } 216 217 public void setSpringDampingRatio(float ratio) { 218 m_dampingRatio = ratio; 219 } 220 221 public float getSpringDampingRatio() { 222 return m_dampingRatio; 223 } 224 225 // pooling 226 private final Vec2 rA = new Vec2(); 227 private final Vec2 rB = new Vec2(); 228 private final Vec2 d = new Vec2(); 229 230 @Override 231 public void initVelocityConstraints(SolverData data) { 232 m_indexA = m_bodyA.m_islandIndex; 233 m_indexB = m_bodyB.m_islandIndex; 234 m_localCenterA.set(m_bodyA.m_sweep.localCenter); 235 m_localCenterB.set(m_bodyB.m_sweep.localCenter); 236 m_invMassA = m_bodyA.m_invMass; 237 m_invMassB = m_bodyB.m_invMass; 238 m_invIA = m_bodyA.m_invI; 239 m_invIB = m_bodyB.m_invI; 240 241 float mA = m_invMassA, mB = m_invMassB; 242 float iA = m_invIA, iB = m_invIB; 243 244 Vec2 cA = data.positions[m_indexA].c; 245 float aA = data.positions[m_indexA].a; 246 Vec2 vA = data.velocities[m_indexA].v; 247 float wA = data.velocities[m_indexA].w; 248 249 Vec2 cB = data.positions[m_indexB].c; 250 float aB = data.positions[m_indexB].a; 251 Vec2 vB = data.velocities[m_indexB].v; 252 float wB = data.velocities[m_indexB].w; 253 254 final Rot qA = pool.popRot(); 255 final Rot qB = pool.popRot(); 256 final Vec2 temp = pool.popVec2(); 257 258 qA.set(aA); 259 qB.set(aB); 260 261 // Compute the effective masses. 262 Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); 263 Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); 264 d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); 265 266 // Point to line constraint 267 { 268 Rot.mulToOut(qA, m_localYAxisA, m_ay); 269 m_sAy = Vec2.cross(temp.set(d).addLocal(rA), m_ay); 270 m_sBy = Vec2.cross(rB, m_ay); 271 272 m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; 273 274 if (m_mass > 0.0f) { 275 m_mass = 1.0f / m_mass; 276 } 277 } 278 279 // Spring constraint 280 m_springMass = 0.0f; 281 m_bias = 0.0f; 282 m_gamma = 0.0f; 283 if (m_frequencyHz > 0.0f) { 284 Rot.mulToOut(qA, m_localXAxisA, m_ax); 285 m_sAx = Vec2.cross(temp.set(d).addLocal(rA), m_ax); 286 m_sBx = Vec2.cross(rB, m_ax); 287 288 float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; 289 290 if (invMass > 0.0f) { 291 m_springMass = 1.0f / invMass; 292 293 float C = Vec2.dot(d, m_ax); 294 295 // Frequency 296 float omega = 2.0f * MathUtils.PI * m_frequencyHz; 297 298 // Damping coefficient 299 float d = 2.0f * m_springMass * m_dampingRatio * omega; 300 301 // Spring stiffness 302 float k = m_springMass * omega * omega; 303 304 // magic formulas 305 float h = data.step.dt; 306 m_gamma = h * (d + h * k); 307 if (m_gamma > 0.0f) { 308 m_gamma = 1.0f / m_gamma; 309 } 310 311 m_bias = C * h * k * m_gamma; 312 313 m_springMass = invMass + m_gamma; 314 if (m_springMass > 0.0f) { 315 m_springMass = 1.0f / m_springMass; 316 } 317 } 318 } else { 319 m_springImpulse = 0.0f; 320 } 321 322 // Rotational motor 323 if (m_enableMotor) { 324 m_motorMass = iA + iB; 325 if (m_motorMass > 0.0f) { 326 m_motorMass = 1.0f / m_motorMass; 327 } 328 } else { 329 m_motorMass = 0.0f; 330 m_motorImpulse = 0.0f; 331 } 332 333 if (data.step.warmStarting) { 334 final Vec2 P = pool.popVec2(); 335 // Account for variable time step. 336 m_impulse *= data.step.dtRatio; 337 m_springImpulse *= data.step.dtRatio; 338 m_motorImpulse *= data.step.dtRatio; 339 340 P.x = m_impulse * m_ay.x + m_springImpulse * m_ax.x; 341 P.y = m_impulse * m_ay.y + m_springImpulse * m_ax.y; 342 float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; 343 float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; 344 345 vA.x -= m_invMassA * P.x; 346 vA.y -= m_invMassA * P.y; 347 wA -= m_invIA * LA; 348 349 vB.x += m_invMassB * P.x; 350 vB.y += m_invMassB * P.y; 351 wB += m_invIB * LB; 352 pool.pushVec2(1); 353 } else { 354 m_impulse = 0.0f; 355 m_springImpulse = 0.0f; 356 m_motorImpulse = 0.0f; 357 } 358 pool.pushRot(2); 359 pool.pushVec2(1); 360 361 // data.velocities[m_indexA].v = vA; 362 data.velocities[m_indexA].w = wA; 363 // data.velocities[m_indexB].v = vB; 364 data.velocities[m_indexB].w = wB; 365 } 366 367 @Override 368 public void solveVelocityConstraints(SolverData data) { 369 float mA = m_invMassA, mB = m_invMassB; 370 float iA = m_invIA, iB = m_invIB; 371 372 Vec2 vA = data.velocities[m_indexA].v; 373 float wA = data.velocities[m_indexA].w; 374 Vec2 vB = data.velocities[m_indexB].v; 375 float wB = data.velocities[m_indexB].w; 376 377 final Vec2 temp = pool.popVec2(); 378 final Vec2 P = pool.popVec2(); 379 380 // Solve spring constraint 381 { 382 float Cdot = Vec2.dot(m_ax, temp.set(vB).subLocal(vA)) + m_sBx * wB - m_sAx * wA; 383 float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); 384 m_springImpulse += impulse; 385 386 P.x = impulse * m_ax.x; 387 P.y = impulse * m_ax.y; 388 float LA = impulse * m_sAx; 389 float LB = impulse * m_sBx; 390 391 vA.x -= mA * P.x; 392 vA.y -= mA * P.y; 393 wA -= iA * LA; 394 395 vB.x += mB * P.x; 396 vB.y += mB * P.y; 397 wB += iB * LB; 398 } 399 400 // Solve rotational motor constraint 401 { 402 float Cdot = wB - wA - m_motorSpeed; 403 float impulse = -m_motorMass * Cdot; 404 405 float oldImpulse = m_motorImpulse; 406 float maxImpulse = data.step.dt * m_maxMotorTorque; 407 m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); 408 impulse = m_motorImpulse - oldImpulse; 409 410 wA -= iA * impulse; 411 wB += iB * impulse; 412 } 413 414 // Solve point to line constraint 415 { 416 float Cdot = Vec2.dot(m_ay, temp.set(vB).subLocal(vA)) + m_sBy * wB - m_sAy * wA; 417 float impulse = -m_mass * Cdot; 418 m_impulse += impulse; 419 420 P.x = impulse * m_ay.x; 421 P.y = impulse * m_ay.y; 422 float LA = impulse * m_sAy; 423 float LB = impulse * m_sBy; 424 425 vA.x -= mA * P.x; 426 vA.y -= mA * P.y; 427 wA -= iA * LA; 428 429 vB.x += mB * P.x; 430 vB.y += mB * P.y; 431 wB += iB * LB; 432 } 433 pool.pushVec2(2); 434 435 // data.velocities[m_indexA].v = vA; 436 data.velocities[m_indexA].w = wA; 437 // data.velocities[m_indexB].v = vB; 438 data.velocities[m_indexB].w = wB; 439 } 440 441 @Override 442 public boolean solvePositionConstraints(SolverData data) { 443 Vec2 cA = data.positions[m_indexA].c; 444 float aA = data.positions[m_indexA].a; 445 Vec2 cB = data.positions[m_indexB].c; 446 float aB = data.positions[m_indexB].a; 447 448 final Rot qA = pool.popRot(); 449 final Rot qB = pool.popRot(); 450 final Vec2 temp = pool.popVec2(); 451 452 qA.set(aA); 453 qB.set(aB); 454 455 Rot.mulToOut(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); 456 Rot.mulToOut(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); 457 d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA); 458 459 Vec2 ay = pool.popVec2(); 460 Rot.mulToOut(qA, m_localYAxisA, ay); 461 462 float sAy = Vec2.cross(temp.set(d).addLocal(rA), ay); 463 float sBy = Vec2.cross(rB, ay); 464 465 float C = Vec2.dot(d, ay); 466 467 float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; 468 469 float impulse; 470 if (k != 0.0f) { 471 impulse = -C / k; 472 } else { 473 impulse = 0.0f; 474 } 475 476 final Vec2 P = pool.popVec2(); 477 P.x = impulse * ay.x; 478 P.y = impulse * ay.y; 479 float LA = impulse * sAy; 480 float LB = impulse * sBy; 481 482 cA.x -= m_invMassA * P.x; 483 cA.y -= m_invMassA * P.y; 484 aA -= m_invIA * LA; 485 cB.x += m_invMassB * P.x; 486 cB.y += m_invMassB * P.y; 487 aB += m_invIB * LB; 488 489 pool.pushVec2(3); 490 pool.pushRot(2); 491 // data.positions[m_indexA].c = cA; 492 data.positions[m_indexA].a = aA; 493 // data.positions[m_indexB].c = cB; 494 data.positions[m_indexB].a = aB; 495 496 return MathUtils.abs(C) <= Settings.linearSlop; 497 } 498 } 499