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 /** 25 * Created at 11:34:45 AM Jan 23, 2011 26 */ 27 package org.jbox2d.dynamics.joints; 28 29 import org.jbox2d.common.Rot; 30 import org.jbox2d.common.Settings; 31 import org.jbox2d.common.Transform; 32 import org.jbox2d.common.Vec2; 33 import org.jbox2d.dynamics.Body; 34 import org.jbox2d.dynamics.SolverData; 35 import org.jbox2d.pooling.IWorldPool; 36 37 //Gear Joint: 38 //C0 = (coordinate1 + ratio * coordinate2)_initial 39 //C = (coordinate1 + ratio * coordinate2) - C0 = 0 40 //J = [J1 ratio * J2] 41 //K = J * invM * JT 42 //= J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T 43 // 44 //Revolute: 45 //coordinate = rotation 46 //Cdot = angularVelocity 47 //J = [0 0 1] 48 //K = J * invM * JT = invI 49 // 50 //Prismatic: 51 //coordinate = dot(p - pg, ug) 52 //Cdot = dot(v + cross(w, r), ug) 53 //J = [ug cross(r, ug)] 54 //K = J * invM * JT = invMass + invI * cross(r, ug)^2 55 56 /** 57 * A gear joint is used to connect two joints together. Either joint can be a revolute or prismatic 58 * joint. You specify a gear ratio to bind the motions together: coordinate1 + ratio * coordinate2 = 59 * constant The ratio can be negative or positive. If one joint is a revolute joint and the other 60 * joint is a prismatic joint, then the ratio will have units of length or units of 1/length. 61 * 62 * @warning The revolute and prismatic joints must be attached to fixed bodies (which must be body1 63 * on those joints). 64 * @warning You have to manually destroy the gear joint if joint1 or joint2 is destroyed. 65 * @author Daniel Murphy 66 */ 67 public class GearJoint extends Joint { 68 69 private final Joint m_joint1; 70 private final Joint m_joint2; 71 72 private final JointType m_typeA; 73 private final JointType m_typeB; 74 75 // Body A is connected to body C 76 // Body B is connected to body D 77 private final Body m_bodyC; 78 private final Body m_bodyD; 79 80 // Solver shared 81 private final Vec2 m_localAnchorA = new Vec2(); 82 private final Vec2 m_localAnchorB = new Vec2(); 83 private final Vec2 m_localAnchorC = new Vec2(); 84 private final Vec2 m_localAnchorD = new Vec2(); 85 86 private final Vec2 m_localAxisC = new Vec2(); 87 private final Vec2 m_localAxisD = new Vec2(); 88 89 private float m_referenceAngleA; 90 private float m_referenceAngleB; 91 92 private float m_constant; 93 private float m_ratio; 94 95 private float m_impulse; 96 97 // Solver temp 98 private int m_indexA, m_indexB, m_indexC, m_indexD; 99 private final Vec2 m_lcA = new Vec2(), m_lcB = new Vec2(), m_lcC = new Vec2(), 100 m_lcD = new Vec2(); 101 private float m_mA, m_mB, m_mC, m_mD; 102 private float m_iA, m_iB, m_iC, m_iD; 103 private final Vec2 m_JvAC = new Vec2(), m_JvBD = new Vec2(); 104 private float m_JwA, m_JwB, m_JwC, m_JwD; 105 private float m_mass; 106 107 protected GearJoint(IWorldPool argWorldPool, GearJointDef def) { 108 super(argWorldPool, def); 109 110 m_joint1 = def.joint1; 111 m_joint2 = def.joint2; 112 113 m_typeA = m_joint1.getType(); 114 m_typeB = m_joint2.getType(); 115 116 assert (m_typeA == JointType.REVOLUTE || m_typeA == JointType.PRISMATIC); 117 assert (m_typeB == JointType.REVOLUTE || m_typeB == JointType.PRISMATIC); 118 119 float coordinateA, coordinateB; 120 121 // TODO_ERIN there might be some problem with the joint edges in Joint. 122 123 m_bodyC = m_joint1.getBodyA(); 124 m_bodyA = m_joint1.getBodyB(); 125 126 // Get geometry of joint1 127 Transform xfA = m_bodyA.m_xf; 128 float aA = m_bodyA.m_sweep.a; 129 Transform xfC = m_bodyC.m_xf; 130 float aC = m_bodyC.m_sweep.a; 131 132 if (m_typeA == JointType.REVOLUTE) { 133 RevoluteJoint revolute = (RevoluteJoint) def.joint1; 134 m_localAnchorC.set(revolute.m_localAnchorA); 135 m_localAnchorA.set(revolute.m_localAnchorB); 136 m_referenceAngleA = revolute.m_referenceAngle; 137 m_localAxisC.setZero(); 138 139 coordinateA = aA - aC - m_referenceAngleA; 140 } else { 141 Vec2 pA = pool.popVec2(); 142 Vec2 temp = pool.popVec2(); 143 PrismaticJoint prismatic = (PrismaticJoint) def.joint1; 144 m_localAnchorC.set(prismatic.m_localAnchorA); 145 m_localAnchorA.set(prismatic.m_localAnchorB); 146 m_referenceAngleA = prismatic.m_referenceAngle; 147 m_localAxisC.set(prismatic.m_localXAxisA); 148 149 Vec2 pC = m_localAnchorC; 150 Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, temp); 151 temp.addLocal(xfA.p).subLocal(xfC.p); 152 Rot.mulTransUnsafe(xfC.q, temp, pA); 153 coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC); 154 pool.pushVec2(2); 155 } 156 157 m_bodyD = m_joint2.getBodyA(); 158 m_bodyB = m_joint2.getBodyB(); 159 160 // Get geometry of joint2 161 Transform xfB = m_bodyB.m_xf; 162 float aB = m_bodyB.m_sweep.a; 163 Transform xfD = m_bodyD.m_xf; 164 float aD = m_bodyD.m_sweep.a; 165 166 if (m_typeB == JointType.REVOLUTE) { 167 RevoluteJoint revolute = (RevoluteJoint) def.joint2; 168 m_localAnchorD.set(revolute.m_localAnchorA); 169 m_localAnchorB.set(revolute.m_localAnchorB); 170 m_referenceAngleB = revolute.m_referenceAngle; 171 m_localAxisD.setZero(); 172 173 coordinateB = aB - aD - m_referenceAngleB; 174 } else { 175 Vec2 pB = pool.popVec2(); 176 Vec2 temp = pool.popVec2(); 177 PrismaticJoint prismatic = (PrismaticJoint) def.joint2; 178 m_localAnchorD.set(prismatic.m_localAnchorA); 179 m_localAnchorB.set(prismatic.m_localAnchorB); 180 m_referenceAngleB = prismatic.m_referenceAngle; 181 m_localAxisD.set(prismatic.m_localXAxisA); 182 183 Vec2 pD = m_localAnchorD; 184 Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, temp); 185 temp.addLocal(xfB.p).subLocal(xfD.p); 186 Rot.mulTransUnsafe(xfD.q, temp, pB); 187 coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); 188 pool.pushVec2(2); 189 } 190 191 m_ratio = def.ratio; 192 193 m_constant = coordinateA + m_ratio * coordinateB; 194 195 m_impulse = 0.0f; 196 } 197 198 @Override 199 public void getAnchorA(Vec2 argOut) { 200 m_bodyA.getWorldPointToOut(m_localAnchorA, argOut); 201 } 202 203 @Override 204 public void getAnchorB(Vec2 argOut) { 205 m_bodyB.getWorldPointToOut(m_localAnchorB, argOut); 206 } 207 208 @Override 209 public void getReactionForce(float inv_dt, Vec2 argOut) { 210 argOut.set(m_JvAC).mulLocal(m_impulse); 211 argOut.mulLocal(inv_dt); 212 } 213 214 @Override 215 public float getReactionTorque(float inv_dt) { 216 float L = m_impulse * m_JwA; 217 return inv_dt * L; 218 } 219 220 public void setRatio(float argRatio) { 221 m_ratio = argRatio; 222 } 223 224 public float getRatio() { 225 return m_ratio; 226 } 227 228 @Override 229 public void initVelocityConstraints(SolverData data) { 230 m_indexA = m_bodyA.m_islandIndex; 231 m_indexB = m_bodyB.m_islandIndex; 232 m_indexC = m_bodyC.m_islandIndex; 233 m_indexD = m_bodyD.m_islandIndex; 234 m_lcA.set(m_bodyA.m_sweep.localCenter); 235 m_lcB.set(m_bodyB.m_sweep.localCenter); 236 m_lcC.set(m_bodyC.m_sweep.localCenter); 237 m_lcD.set(m_bodyD.m_sweep.localCenter); 238 m_mA = m_bodyA.m_invMass; 239 m_mB = m_bodyB.m_invMass; 240 m_mC = m_bodyC.m_invMass; 241 m_mD = m_bodyD.m_invMass; 242 m_iA = m_bodyA.m_invI; 243 m_iB = m_bodyB.m_invI; 244 m_iC = m_bodyC.m_invI; 245 m_iD = m_bodyD.m_invI; 246 247 // Vec2 cA = data.positions[m_indexA].c; 248 float aA = data.positions[m_indexA].a; 249 Vec2 vA = data.velocities[m_indexA].v; 250 float wA = data.velocities[m_indexA].w; 251 252 // Vec2 cB = data.positions[m_indexB].c; 253 float aB = data.positions[m_indexB].a; 254 Vec2 vB = data.velocities[m_indexB].v; 255 float wB = data.velocities[m_indexB].w; 256 257 // Vec2 cC = data.positions[m_indexC].c; 258 float aC = data.positions[m_indexC].a; 259 Vec2 vC = data.velocities[m_indexC].v; 260 float wC = data.velocities[m_indexC].w; 261 262 // Vec2 cD = data.positions[m_indexD].c; 263 float aD = data.positions[m_indexD].a; 264 Vec2 vD = data.velocities[m_indexD].v; 265 float wD = data.velocities[m_indexD].w; 266 267 Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot(); 268 qA.set(aA); 269 qB.set(aB); 270 qC.set(aC); 271 qD.set(aD); 272 273 m_mass = 0.0f; 274 275 Vec2 temp = pool.popVec2(); 276 277 if (m_typeA == JointType.REVOLUTE) { 278 m_JvAC.setZero(); 279 m_JwA = 1.0f; 280 m_JwC = 1.0f; 281 m_mass += m_iA + m_iC; 282 } else { 283 Vec2 rC = pool.popVec2(); 284 Vec2 rA = pool.popVec2(); 285 Rot.mulToOutUnsafe(qC, m_localAxisC, m_JvAC); 286 Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC); 287 Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA); 288 m_JwC = Vec2.cross(rC, m_JvAC); 289 m_JwA = Vec2.cross(rA, m_JvAC); 290 m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; 291 pool.pushVec2(2); 292 } 293 294 if (m_typeB == JointType.REVOLUTE) { 295 m_JvBD.setZero(); 296 m_JwB = m_ratio; 297 m_JwD = m_ratio; 298 m_mass += m_ratio * m_ratio * (m_iB + m_iD); 299 } else { 300 Vec2 u = pool.popVec2(); 301 Vec2 rD = pool.popVec2(); 302 Vec2 rB = pool.popVec2(); 303 Rot.mulToOutUnsafe(qD, m_localAxisD, u); 304 Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD); 305 Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB); 306 m_JvBD.set(u).mulLocal(m_ratio); 307 m_JwD = m_ratio * Vec2.cross(rD, u); 308 m_JwB = m_ratio * Vec2.cross(rB, u); 309 m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; 310 pool.pushVec2(3); 311 } 312 313 // Compute effective mass. 314 m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; 315 316 if (data.step.warmStarting) { 317 vA.x += (m_mA * m_impulse) * m_JvAC.x; 318 vA.y += (m_mA * m_impulse) * m_JvAC.y; 319 wA += m_iA * m_impulse * m_JwA; 320 321 vB.x += (m_mB * m_impulse) * m_JvBD.x; 322 vB.y += (m_mB * m_impulse) * m_JvBD.y; 323 wB += m_iB * m_impulse * m_JwB; 324 325 vC.x -= (m_mC * m_impulse) * m_JvAC.x; 326 vC.y -= (m_mC * m_impulse) * m_JvAC.y; 327 wC -= m_iC * m_impulse * m_JwC; 328 329 vD.x -= (m_mD * m_impulse) * m_JvBD.x; 330 vD.y -= (m_mD * m_impulse) * m_JvBD.y; 331 wD -= m_iD * m_impulse * m_JwD; 332 } else { 333 m_impulse = 0.0f; 334 } 335 pool.pushVec2(1); 336 pool.pushRot(4); 337 338 // data.velocities[m_indexA].v = vA; 339 data.velocities[m_indexA].w = wA; 340 // data.velocities[m_indexB].v = vB; 341 data.velocities[m_indexB].w = wB; 342 // data.velocities[m_indexC].v = vC; 343 data.velocities[m_indexC].w = wC; 344 // data.velocities[m_indexD].v = vD; 345 data.velocities[m_indexD].w = wD; 346 } 347 348 @Override 349 public void solveVelocityConstraints(SolverData data) { 350 Vec2 vA = data.velocities[m_indexA].v; 351 float wA = data.velocities[m_indexA].w; 352 Vec2 vB = data.velocities[m_indexB].v; 353 float wB = data.velocities[m_indexB].w; 354 Vec2 vC = data.velocities[m_indexC].v; 355 float wC = data.velocities[m_indexC].w; 356 Vec2 vD = data.velocities[m_indexD].v; 357 float wD = data.velocities[m_indexD].w; 358 359 Vec2 temp1 = pool.popVec2(); 360 Vec2 temp2 = pool.popVec2(); 361 float Cdot = 362 Vec2.dot(m_JvAC, temp1.set(vA).subLocal(vC)) + Vec2.dot(m_JvBD, temp2.set(vB).subLocal(vD)); 363 Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); 364 pool.pushVec2(2); 365 366 float impulse = -m_mass * Cdot; 367 m_impulse += impulse; 368 369 vA.x += (m_mA * impulse) * m_JvAC.x; 370 vA.y += (m_mA * impulse) * m_JvAC.y; 371 wA += m_iA * impulse * m_JwA; 372 373 vB.x += (m_mB * impulse) * m_JvBD.x; 374 vB.y += (m_mB * impulse) * m_JvBD.y; 375 wB += m_iB * impulse * m_JwB; 376 377 vC.x -= (m_mC * impulse) * m_JvAC.x; 378 vC.y -= (m_mC * impulse) * m_JvAC.y; 379 wC -= m_iC * impulse * m_JwC; 380 381 vD.x -= (m_mD * impulse) * m_JvBD.x; 382 vD.y -= (m_mD * impulse) * m_JvBD.y; 383 wD -= m_iD * impulse * m_JwD; 384 385 386 // data.velocities[m_indexA].v = vA; 387 data.velocities[m_indexA].w = wA; 388 // data.velocities[m_indexB].v = vB; 389 data.velocities[m_indexB].w = wB; 390 // data.velocities[m_indexC].v = vC; 391 data.velocities[m_indexC].w = wC; 392 // data.velocities[m_indexD].v = vD; 393 data.velocities[m_indexD].w = wD; 394 } 395 396 public Joint getJoint1() { 397 return m_joint1; 398 } 399 400 public Joint getJoint2() { 401 return m_joint2; 402 } 403 404 @Override 405 public boolean solvePositionConstraints(SolverData data) { 406 Vec2 cA = data.positions[m_indexA].c; 407 float aA = data.positions[m_indexA].a; 408 Vec2 cB = data.positions[m_indexB].c; 409 float aB = data.positions[m_indexB].a; 410 Vec2 cC = data.positions[m_indexC].c; 411 float aC = data.positions[m_indexC].a; 412 Vec2 cD = data.positions[m_indexD].c; 413 float aD = data.positions[m_indexD].a; 414 415 Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot(); 416 qA.set(aA); 417 qB.set(aB); 418 qC.set(aC); 419 qD.set(aD); 420 421 float linearError = 0.0f; 422 423 float coordinateA, coordinateB; 424 425 Vec2 temp = pool.popVec2(); 426 Vec2 JvAC = pool.popVec2(); 427 Vec2 JvBD = pool.popVec2(); 428 float JwA, JwB, JwC, JwD; 429 float mass = 0.0f; 430 431 if (m_typeA == JointType.REVOLUTE) { 432 JvAC.setZero(); 433 JwA = 1.0f; 434 JwC = 1.0f; 435 mass += m_iA + m_iC; 436 437 coordinateA = aA - aC - m_referenceAngleA; 438 } else { 439 Vec2 rC = pool.popVec2(); 440 Vec2 rA = pool.popVec2(); 441 Vec2 pC = pool.popVec2(); 442 Vec2 pA = pool.popVec2(); 443 Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC); 444 Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC); 445 Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA); 446 JwC = Vec2.cross(rC, JvAC); 447 JwA = Vec2.cross(rA, JvAC); 448 mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; 449 450 pC.set(m_localAnchorC).subLocal(m_lcC); 451 Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA); 452 coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC); 453 pool.pushVec2(4); 454 } 455 456 if (m_typeB == JointType.REVOLUTE) { 457 JvBD.setZero(); 458 JwB = m_ratio; 459 JwD = m_ratio; 460 mass += m_ratio * m_ratio * (m_iB + m_iD); 461 462 coordinateB = aB - aD - m_referenceAngleB; 463 } else { 464 Vec2 u = pool.popVec2(); 465 Vec2 rD = pool.popVec2(); 466 Vec2 rB = pool.popVec2(); 467 Vec2 pD = pool.popVec2(); 468 Vec2 pB = pool.popVec2(); 469 Rot.mulToOutUnsafe(qD, m_localAxisD, u); 470 Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD); 471 Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB); 472 JvBD.set(u).mulLocal(m_ratio); 473 JwD = Vec2.cross(rD, u); 474 JwB = Vec2.cross(rB, u); 475 mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; 476 477 pD.set(m_localAnchorD).subLocal(m_lcD); 478 Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB); 479 coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); 480 pool.pushVec2(5); 481 } 482 483 float C = (coordinateA + m_ratio * coordinateB) - m_constant; 484 485 float impulse = 0.0f; 486 if (mass > 0.0f) { 487 impulse = -C / mass; 488 } 489 pool.pushVec2(3); 490 pool.pushRot(4); 491 492 cA.x += (m_mA * impulse) * JvAC.x; 493 cA.y += (m_mA * impulse) * JvAC.y; 494 aA += m_iA * impulse * JwA; 495 496 cB.x += (m_mB * impulse) * JvBD.x; 497 cB.y += (m_mB * impulse) * JvBD.y; 498 aB += m_iB * impulse * JwB; 499 500 cC.x -= (m_mC * impulse) * JvAC.x; 501 cC.y -= (m_mC * impulse) * JvAC.y; 502 aC -= m_iC * impulse * JwC; 503 504 cD.x -= (m_mD * impulse) * JvBD.x; 505 cD.y -= (m_mD * impulse) * JvBD.y; 506 aD -= m_iD * impulse * JwD; 507 508 // data.positions[m_indexA].c = cA; 509 data.positions[m_indexA].a = aA; 510 // data.positions[m_indexB].c = cB; 511 data.positions[m_indexB].a = aB; 512 // data.positions[m_indexC].c = cC; 513 data.positions[m_indexC].a = aC; 514 // data.positions[m_indexD].c = cD; 515 data.positions[m_indexD].a = aD; 516 517 // TODO_ERIN not implemented 518 return linearError < Settings.linearSlop; 519 } 520 } 521