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.Mat22; 27 import org.jbox2d.common.Mat33; 28 import org.jbox2d.common.MathUtils; 29 import org.jbox2d.common.Rot; 30 import org.jbox2d.common.Settings; 31 import org.jbox2d.common.Vec2; 32 import org.jbox2d.common.Vec3; 33 import org.jbox2d.dynamics.Body; 34 import org.jbox2d.dynamics.SolverData; 35 import org.jbox2d.pooling.IWorldPool; 36 37 //Linear constraint (point-to-line) 38 //d = p2 - p1 = x2 + r2 - x1 - r1 39 //C = dot(perp, d) 40 //Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) 41 // = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) 42 //J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] 43 // 44 //Angular constraint 45 //C = a2 - a1 + a_initial 46 //Cdot = w2 - w1 47 //J = [0 0 -1 0 0 1] 48 // 49 //K = J * invM * JT 50 // 51 //J = [-a -s1 a s2] 52 // [0 -1 0 1] 53 //a = perp 54 //s1 = cross(d + r1, a) = cross(p2 - x1, a) 55 //s2 = cross(r2, a) = cross(p2 - x2, a) 56 57 58 //Motor/Limit linear constraint 59 //C = dot(ax1, d) 60 //Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) 61 //J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] 62 63 //Block Solver 64 //We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even 65 //when the mass has poor distribution (leading to large torques about the joint anchor points). 66 // 67 //The Jacobian has 3 rows: 68 //J = [-uT -s1 uT s2] // linear 69 // [0 -1 0 1] // angular 70 // [-vT -a1 vT a2] // limit 71 // 72 //u = perp 73 //v = axis 74 //s1 = cross(d + r1, u), s2 = cross(r2, u) 75 //a1 = cross(d + r1, v), a2 = cross(r2, v) 76 77 //M * (v2 - v1) = JT * df 78 //J * v2 = bias 79 // 80 //v2 = v1 + invM * JT * df 81 //J * (v1 + invM * JT * df) = bias 82 //K * df = bias - J * v1 = -Cdot 83 //K = J * invM * JT 84 //Cdot = J * v1 - bias 85 // 86 //Now solve for f2. 87 //df = f2 - f1 88 //K * (f2 - f1) = -Cdot 89 //f2 = invK * (-Cdot) + f1 90 // 91 //Clamp accumulated limit impulse. 92 //lower: f2(3) = max(f2(3), 0) 93 //upper: f2(3) = min(f2(3), 0) 94 // 95 //Solve for correct f2(1:2) 96 //K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 97 // = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) 98 //K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) 99 //f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) 100 // 101 //Now compute impulse to be applied: 102 //df = f2 - f1 103 104 /** 105 * A prismatic joint. This joint provides one degree of freedom: translation along an axis fixed in 106 * bodyA. Relative rotation is prevented. You can use a joint limit to restrict the range of motion 107 * and a joint motor to drive the motion or to model joint friction. 108 * 109 * @author Daniel 110 */ 111 public class PrismaticJoint extends Joint { 112 113 // Solver shared 114 protected final Vec2 m_localAnchorA; 115 protected final Vec2 m_localAnchorB; 116 protected final Vec2 m_localXAxisA; 117 protected final Vec2 m_localYAxisA; 118 protected float m_referenceAngle; 119 private final Vec3 m_impulse; 120 private float m_motorImpulse; 121 private float m_lowerTranslation; 122 private float m_upperTranslation; 123 private float m_maxMotorForce; 124 private float m_motorSpeed; 125 private boolean m_enableLimit; 126 private boolean m_enableMotor; 127 private LimitState m_limitState; 128 129 // Solver temp 130 private int m_indexA; 131 private int m_indexB; 132 private final Vec2 m_localCenterA = new Vec2(); 133 private final Vec2 m_localCenterB = new Vec2(); 134 private float m_invMassA; 135 private float m_invMassB; 136 private float m_invIA; 137 private float m_invIB; 138 private final Vec2 m_axis, m_perp; 139 private float m_s1, m_s2; 140 private float m_a1, m_a2; 141 private final Mat33 m_K; 142 private float m_motorMass; // effective mass for motor/limit translational constraint. 143 144 protected PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) { 145 super(argWorld, def); 146 m_localAnchorA = new Vec2(def.localAnchorA); 147 m_localAnchorB = new Vec2(def.localAnchorB); 148 m_localXAxisA = new Vec2(def.localAxisA); 149 m_localXAxisA.normalize(); 150 m_localYAxisA = new Vec2(); 151 Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA); 152 m_referenceAngle = def.referenceAngle; 153 154 m_impulse = new Vec3(); 155 m_motorMass = 0.0f; 156 m_motorImpulse = 0.0f; 157 158 m_lowerTranslation = def.lowerTranslation; 159 m_upperTranslation = def.upperTranslation; 160 m_maxMotorForce = def.maxMotorForce; 161 m_motorSpeed = def.motorSpeed; 162 m_enableLimit = def.enableLimit; 163 m_enableMotor = def.enableMotor; 164 m_limitState = LimitState.INACTIVE; 165 166 m_K = new Mat33(); 167 m_axis = new Vec2(); 168 m_perp = new Vec2(); 169 } 170 171 public Vec2 getLocalAnchorA() { 172 return m_localAnchorA; 173 } 174 175 public Vec2 getLocalAnchorB() { 176 return m_localAnchorB; 177 } 178 179 @Override 180 public void getAnchorA(Vec2 argOut) { 181 m_bodyA.getWorldPointToOut(m_localAnchorA, argOut); 182 } 183 184 @Override 185 public void getAnchorB(Vec2 argOut) { 186 m_bodyB.getWorldPointToOut(m_localAnchorB, argOut); 187 } 188 189 @Override 190 public void getReactionForce(float inv_dt, Vec2 argOut) { 191 Vec2 temp = pool.popVec2(); 192 temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z); 193 argOut.set(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt); 194 pool.pushVec2(1); 195 } 196 197 @Override 198 public float getReactionTorque(float inv_dt) { 199 return inv_dt * m_impulse.y; 200 } 201 202 /** 203 * Get the current joint translation, usually in meters. 204 */ 205 public float getJointSpeed() { 206 Body bA = m_bodyA; 207 Body bB = m_bodyB; 208 209 Vec2 temp = pool.popVec2(); 210 Vec2 rA = pool.popVec2(); 211 Vec2 rB = pool.popVec2(); 212 Vec2 p1 = pool.popVec2(); 213 Vec2 p2 = pool.popVec2(); 214 Vec2 d = pool.popVec2(); 215 Vec2 axis = pool.popVec2(); 216 Vec2 temp2 = pool.popVec2(); 217 Vec2 temp3 = pool.popVec2(); 218 219 temp.set(m_localAnchorA).subLocal(bA.m_sweep.localCenter); 220 Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA); 221 222 temp.set(m_localAnchorB).subLocal(bB.m_sweep.localCenter); 223 Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB); 224 225 p1.set(bA.m_sweep.c).addLocal(rA); 226 p2.set(bB.m_sweep.c).addLocal(rB); 227 228 d.set(p2).subLocal(p1); 229 Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis); 230 231 Vec2 vA = bA.m_linearVelocity; 232 Vec2 vB = bB.m_linearVelocity; 233 float wA = bA.m_angularVelocity; 234 float wB = bB.m_angularVelocity; 235 236 237 Vec2.crossToOutUnsafe(wA, axis, temp); 238 Vec2.crossToOutUnsafe(wB, rB, temp2); 239 Vec2.crossToOutUnsafe(wA, rA, temp3); 240 241 temp2.addLocal(vB).subLocal(vA).subLocal(temp3); 242 float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2); 243 244 pool.pushVec2(9); 245 246 return speed; 247 } 248 249 public float getJointTranslation() { 250 Vec2 pA = pool.popVec2(), pB = pool.popVec2(), axis = pool.popVec2(); 251 m_bodyA.getWorldPointToOut(m_localAnchorA, pA); 252 m_bodyB.getWorldPointToOut(m_localAnchorB, pB); 253 m_bodyA.getWorldVectorToOutUnsafe(m_localXAxisA, axis); 254 pB.subLocal(pA); 255 float translation = Vec2.dot(pB, axis); 256 pool.pushVec2(3); 257 return translation; 258 } 259 260 /** 261 * Is the joint limit enabled? 262 * 263 * @return 264 */ 265 public boolean isLimitEnabled() { 266 return m_enableLimit; 267 } 268 269 /** 270 * Enable/disable the joint limit. 271 * 272 * @param flag 273 */ 274 public void enableLimit(boolean flag) { 275 if (flag != m_enableLimit) { 276 m_bodyA.setAwake(true); 277 m_bodyB.setAwake(true); 278 m_enableLimit = flag; 279 m_impulse.z = 0.0f; 280 } 281 } 282 283 /** 284 * Get the lower joint limit, usually in meters. 285 * 286 * @return 287 */ 288 public float getLowerLimit() { 289 return m_lowerTranslation; 290 } 291 292 /** 293 * Get the upper joint limit, usually in meters. 294 * 295 * @return 296 */ 297 public float getUpperLimit() { 298 return m_upperTranslation; 299 } 300 301 /** 302 * Set the joint limits, usually in meters. 303 * 304 * @param lower 305 * @param upper 306 */ 307 public void setLimits(float lower, float upper) { 308 assert (lower <= upper); 309 if (lower != m_lowerTranslation || upper != m_upperTranslation) { 310 m_bodyA.setAwake(true); 311 m_bodyB.setAwake(true); 312 m_lowerTranslation = lower; 313 m_upperTranslation = upper; 314 m_impulse.z = 0.0f; 315 } 316 } 317 318 /** 319 * Is the joint motor enabled? 320 * 321 * @return 322 */ 323 public boolean isMotorEnabled() { 324 return m_enableMotor; 325 } 326 327 /** 328 * Enable/disable the joint motor. 329 * 330 * @param flag 331 */ 332 public void enableMotor(boolean flag) { 333 m_bodyA.setAwake(true); 334 m_bodyB.setAwake(true); 335 m_enableMotor = flag; 336 } 337 338 /** 339 * Set the motor speed, usually in meters per second. 340 * 341 * @param speed 342 */ 343 public void setMotorSpeed(float speed) { 344 m_bodyA.setAwake(true); 345 m_bodyB.setAwake(true); 346 m_motorSpeed = speed; 347 } 348 349 /** 350 * Get the motor speed, usually in meters per second. 351 * 352 * @return 353 */ 354 public float getMotorSpeed() { 355 return m_motorSpeed; 356 } 357 358 /** 359 * Set the maximum motor force, usually in N. 360 * 361 * @param force 362 */ 363 public void setMaxMotorForce(float force) { 364 m_bodyA.setAwake(true); 365 m_bodyB.setAwake(true); 366 m_maxMotorForce = force; 367 } 368 369 /** 370 * Get the current motor force, usually in N. 371 * 372 * @param inv_dt 373 * @return 374 */ 375 public float getMotorForce(float inv_dt) { 376 return m_motorImpulse * inv_dt; 377 } 378 379 public float getMaxMotorForce() { 380 return m_maxMotorForce; 381 } 382 383 public float getReferenceAngle() { 384 return m_referenceAngle; 385 } 386 387 public Vec2 getLocalAxisA() { 388 return m_localXAxisA; 389 } 390 391 @Override 392 public void initVelocityConstraints(final SolverData data) { 393 m_indexA = m_bodyA.m_islandIndex; 394 m_indexB = m_bodyB.m_islandIndex; 395 m_localCenterA.set(m_bodyA.m_sweep.localCenter); 396 m_localCenterB.set(m_bodyB.m_sweep.localCenter); 397 m_invMassA = m_bodyA.m_invMass; 398 m_invMassB = m_bodyB.m_invMass; 399 m_invIA = m_bodyA.m_invI; 400 m_invIB = m_bodyB.m_invI; 401 402 Vec2 cA = data.positions[m_indexA].c; 403 float aA = data.positions[m_indexA].a; 404 Vec2 vA = data.velocities[m_indexA].v; 405 float wA = data.velocities[m_indexA].w; 406 407 Vec2 cB = data.positions[m_indexB].c; 408 float aB = data.positions[m_indexB].a; 409 Vec2 vB = data.velocities[m_indexB].v; 410 float wB = data.velocities[m_indexB].w; 411 412 final Rot qA = pool.popRot(); 413 final Rot qB = pool.popRot(); 414 final Vec2 d = pool.popVec2(); 415 final Vec2 temp = pool.popVec2(); 416 final Vec2 rA = pool.popVec2(); 417 final Vec2 rB = pool.popVec2(); 418 419 qA.set(aA); 420 qB.set(aB); 421 422 // Compute the effective masses. 423 Rot.mulToOutUnsafe(qA, d.set(m_localAnchorA).subLocal(m_localCenterA), rA); 424 Rot.mulToOutUnsafe(qB, d.set(m_localAnchorB).subLocal(m_localCenterB), rB); 425 d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA); 426 427 float mA = m_invMassA, mB = m_invMassB; 428 float iA = m_invIA, iB = m_invIB; 429 430 // Compute motor Jacobian and effective mass. 431 { 432 Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis); 433 temp.set(d).addLocal(rA); 434 m_a1 = Vec2.cross(temp, m_axis); 435 m_a2 = Vec2.cross(rB, m_axis); 436 437 m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; 438 if (m_motorMass > 0.0f) { 439 m_motorMass = 1.0f / m_motorMass; 440 } 441 } 442 443 // Prismatic constraint. 444 { 445 Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp); 446 447 temp.set(d).addLocal(rA); 448 m_s1 = Vec2.cross(temp, m_perp); 449 m_s2 = Vec2.cross(rB, m_perp); 450 451 float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; 452 float k12 = iA * m_s1 + iB * m_s2; 453 float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; 454 float k22 = iA + iB; 455 if (k22 == 0.0f) { 456 // For bodies with fixed rotation. 457 k22 = 1.0f; 458 } 459 float k23 = iA * m_a1 + iB * m_a2; 460 float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; 461 462 m_K.ex.set(k11, k12, k13); 463 m_K.ey.set(k12, k22, k23); 464 m_K.ez.set(k13, k23, k33); 465 } 466 467 // Compute motor and limit terms. 468 if (m_enableLimit) { 469 470 float jointTranslation = Vec2.dot(m_axis, d); 471 if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) { 472 m_limitState = LimitState.EQUAL; 473 } else if (jointTranslation <= m_lowerTranslation) { 474 if (m_limitState != LimitState.AT_LOWER) { 475 m_limitState = LimitState.AT_LOWER; 476 m_impulse.z = 0.0f; 477 } 478 } else if (jointTranslation >= m_upperTranslation) { 479 if (m_limitState != LimitState.AT_UPPER) { 480 m_limitState = LimitState.AT_UPPER; 481 m_impulse.z = 0.0f; 482 } 483 } else { 484 m_limitState = LimitState.INACTIVE; 485 m_impulse.z = 0.0f; 486 } 487 } else { 488 m_limitState = LimitState.INACTIVE; 489 m_impulse.z = 0.0f; 490 } 491 492 if (m_enableMotor == false) { 493 m_motorImpulse = 0.0f; 494 } 495 496 if (data.step.warmStarting) { 497 // Account for variable time step. 498 m_impulse.mulLocal(data.step.dtRatio); 499 m_motorImpulse *= data.step.dtRatio; 500 501 final Vec2 P = pool.popVec2(); 502 temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z); 503 P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp); 504 505 float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; 506 float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; 507 508 vA.x -= mA * P.x; 509 vA.y -= mA * P.y; 510 wA -= iA * LA; 511 512 vB.x += mB * P.x; 513 vB.y += mB * P.y; 514 wB += iB * LB; 515 516 pool.pushVec2(1); 517 } else { 518 m_impulse.setZero(); 519 m_motorImpulse = 0.0f; 520 } 521 522 // data.velocities[m_indexA].v.set(vA); 523 data.velocities[m_indexA].w = wA; 524 // data.velocities[m_indexB].v.set(vB); 525 data.velocities[m_indexB].w = wB; 526 527 pool.pushRot(2); 528 pool.pushVec2(4); 529 } 530 531 @Override 532 public void solveVelocityConstraints(final SolverData data) { 533 Vec2 vA = data.velocities[m_indexA].v; 534 float wA = data.velocities[m_indexA].w; 535 Vec2 vB = data.velocities[m_indexB].v; 536 float wB = data.velocities[m_indexB].w; 537 538 float mA = m_invMassA, mB = m_invMassB; 539 float iA = m_invIA, iB = m_invIB; 540 541 final Vec2 temp = pool.popVec2(); 542 543 // Solve linear motor constraint. 544 if (m_enableMotor && m_limitState != LimitState.EQUAL) { 545 temp.set(vB).subLocal(vA); 546 float Cdot = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA; 547 float impulse = m_motorMass * (m_motorSpeed - Cdot); 548 float oldImpulse = m_motorImpulse; 549 float maxImpulse = data.step.dt * m_maxMotorForce; 550 m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); 551 impulse = m_motorImpulse - oldImpulse; 552 553 final Vec2 P = pool.popVec2(); 554 P.set(m_axis).mulLocal(impulse); 555 float LA = impulse * m_a1; 556 float LB = impulse * m_a2; 557 558 vA.x -= mA * P.x; 559 vA.y -= mA * P.y; 560 wA -= iA * LA; 561 562 vB.x += mB * P.x; 563 vB.y += mB * P.y; 564 wB += iB * LB; 565 566 pool.pushVec2(1); 567 } 568 569 final Vec2 Cdot1 = pool.popVec2(); 570 temp.set(vB).subLocal(vA); 571 Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * wB - m_s1 * wA; 572 Cdot1.y = wB - wA; 573 // System.out.println(Cdot1); 574 575 if (m_enableLimit && m_limitState != LimitState.INACTIVE) { 576 // Solve prismatic and limit constraint in block form. 577 float Cdot2; 578 temp.set(vB).subLocal(vA); 579 Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA; 580 581 final Vec3 Cdot = pool.popVec3(); 582 Cdot.set(Cdot1.x, Cdot1.y, Cdot2); 583 584 final Vec3 f1 = pool.popVec3(); 585 final Vec3 df = pool.popVec3(); 586 587 f1.set(m_impulse); 588 m_K.solve33ToOut(Cdot.negateLocal(), df); 589 // Cdot.negateLocal(); not used anymore 590 m_impulse.addLocal(df); 591 592 if (m_limitState == LimitState.AT_LOWER) { 593 m_impulse.z = MathUtils.max(m_impulse.z, 0.0f); 594 } else if (m_limitState == LimitState.AT_UPPER) { 595 m_impulse.z = MathUtils.min(m_impulse.z, 0.0f); 596 } 597 598 // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + 599 // f1(1:2) 600 final Vec2 b = pool.popVec2(); 601 final Vec2 f2r = pool.popVec2(); 602 603 temp.set(m_K.ez.x, m_K.ez.y).mulLocal(m_impulse.z - f1.z); 604 b.set(Cdot1).negateLocal().subLocal(temp); 605 606 m_K.solve22ToOut(b, f2r); 607 f2r.addLocal(f1.x, f1.y); 608 m_impulse.x = f2r.x; 609 m_impulse.y = f2r.y; 610 611 df.set(m_impulse).subLocal(f1); 612 613 final Vec2 P = pool.popVec2(); 614 temp.set(m_axis).mulLocal(df.z); 615 P.set(m_perp).mulLocal(df.x).addLocal(temp); 616 617 float LA = df.x * m_s1 + df.y + df.z * m_a1; 618 float LB = df.x * m_s2 + df.y + df.z * m_a2; 619 620 vA.x -= mA * P.x; 621 vA.y -= mA * P.y; 622 wA -= iA * LA; 623 624 vB.x += mB * P.x; 625 vB.y += mB * P.y; 626 wB += iB * LB; 627 628 pool.pushVec2(3); 629 pool.pushVec3(3); 630 } else { 631 // Limit is inactive, just solve the prismatic constraint in block form. 632 final Vec2 df = pool.popVec2(); 633 m_K.solve22ToOut(Cdot1.negateLocal(), df); 634 Cdot1.negateLocal(); 635 636 m_impulse.x += df.x; 637 m_impulse.y += df.y; 638 639 final Vec2 P = pool.popVec2(); 640 P.set(m_perp).mulLocal(df.x); 641 float LA = df.x * m_s1 + df.y; 642 float LB = df.x * m_s2 + df.y; 643 644 vA.x -= mA * P.x; 645 vA.y -= mA * P.y; 646 wA -= iA * LA; 647 648 vB.x += mB * P.x; 649 vB.y += mB * P.y; 650 wB += iB * LB; 651 652 pool.pushVec2(2); 653 } 654 655 // data.velocities[m_indexA].v.set(vA); 656 data.velocities[m_indexA].w = wA; 657 // data.velocities[m_indexB].v.set(vB); 658 data.velocities[m_indexB].w = wB; 659 660 pool.pushVec2(2); 661 } 662 663 664 @Override 665 public boolean solvePositionConstraints(final SolverData data) { 666 667 final Rot qA = pool.popRot(); 668 final Rot qB = pool.popRot(); 669 final Vec2 rA = pool.popVec2(); 670 final Vec2 rB = pool.popVec2(); 671 final Vec2 d = pool.popVec2(); 672 final Vec2 axis = pool.popVec2(); 673 final Vec2 perp = pool.popVec2(); 674 final Vec2 temp = pool.popVec2(); 675 final Vec2 C1 = pool.popVec2(); 676 677 final Vec3 impulse = pool.popVec3(); 678 679 Vec2 cA = data.positions[m_indexA].c; 680 float aA = data.positions[m_indexA].a; 681 Vec2 cB = data.positions[m_indexB].c; 682 float aB = data.positions[m_indexB].a; 683 684 qA.set(aA); 685 qB.set(aB); 686 687 float mA = m_invMassA, mB = m_invMassB; 688 float iA = m_invIA, iB = m_invIB; 689 690 // Compute fresh Jacobians 691 Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); 692 Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); 693 d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); 694 695 Rot.mulToOutUnsafe(qA, m_localXAxisA, axis); 696 float a1 = Vec2.cross(temp.set(d).addLocal(rA), axis); 697 float a2 = Vec2.cross(rB, axis); 698 Rot.mulToOutUnsafe(qA, m_localYAxisA, perp); 699 700 float s1 = Vec2.cross(temp.set(d).addLocal(rA), perp); 701 float s2 = Vec2.cross(rB, perp); 702 703 C1.x = Vec2.dot(perp, d); 704 C1.y = aB - aA - m_referenceAngle; 705 706 float linearError = MathUtils.abs(C1.x); 707 float angularError = MathUtils.abs(C1.y); 708 709 boolean active = false; 710 float C2 = 0.0f; 711 if (m_enableLimit) { 712 float translation = Vec2.dot(axis, d); 713 if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) { 714 // Prevent large angular corrections 715 C2 = 716 MathUtils.clamp(translation, -Settings.maxLinearCorrection, 717 Settings.maxLinearCorrection); 718 linearError = MathUtils.max(linearError, MathUtils.abs(translation)); 719 active = true; 720 } else if (translation <= m_lowerTranslation) { 721 // Prevent large linear corrections and allow some slop. 722 C2 = 723 MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop, 724 -Settings.maxLinearCorrection, 0.0f); 725 linearError = MathUtils.max(linearError, m_lowerTranslation - translation); 726 active = true; 727 } else if (translation >= m_upperTranslation) { 728 // Prevent large linear corrections and allow some slop. 729 C2 = 730 MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0f, 731 Settings.maxLinearCorrection); 732 linearError = MathUtils.max(linearError, translation - m_upperTranslation); 733 active = true; 734 } 735 } 736 737 if (active) { 738 float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; 739 float k12 = iA * s1 + iB * s2; 740 float k13 = iA * s1 * a1 + iB * s2 * a2; 741 float k22 = iA + iB; 742 if (k22 == 0.0f) { 743 // For fixed rotation 744 k22 = 1.0f; 745 } 746 float k23 = iA * a1 + iB * a2; 747 float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; 748 749 final Mat33 K = pool.popMat33(); 750 K.ex.set(k11, k12, k13); 751 K.ey.set(k12, k22, k23); 752 K.ez.set(k13, k23, k33); 753 754 final Vec3 C = pool.popVec3(); 755 C.x = C1.x; 756 C.y = C1.y; 757 C.z = C2; 758 759 K.solve33ToOut(C.negateLocal(), impulse); 760 pool.pushVec3(1); 761 pool.pushMat33(1); 762 } else { 763 float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; 764 float k12 = iA * s1 + iB * s2; 765 float k22 = iA + iB; 766 if (k22 == 0.0f) { 767 k22 = 1.0f; 768 } 769 770 final Mat22 K = pool.popMat22(); 771 K.ex.set(k11, k12); 772 K.ey.set(k12, k22); 773 774 // temp is impulse1 775 K.solveToOut(C1.negateLocal(), temp); 776 C1.negateLocal(); 777 778 impulse.x = temp.x; 779 impulse.y = temp.y; 780 impulse.z = 0.0f; 781 782 pool.pushMat22(1); 783 } 784 785 float Px = impulse.x * perp.x + impulse.z * axis.x; 786 float Py = impulse.x * perp.y + impulse.z * axis.y; 787 float LA = impulse.x * s1 + impulse.y + impulse.z * a1; 788 float LB = impulse.x * s2 + impulse.y + impulse.z * a2; 789 790 cA.x -= mA * Px; 791 cA.y -= mA * Py; 792 aA -= iA * LA; 793 cB.x += mB * Px; 794 cB.y += mB * Py; 795 aB += iB * LB; 796 797 // data.positions[m_indexA].c.set(cA); 798 data.positions[m_indexA].a = aA; 799 // data.positions[m_indexB].c.set(cB); 800 data.positions[m_indexB].a = aB; 801 802 pool.pushVec2(7); 803 pool.pushVec3(1); 804 pool.pushRot(2); 805 806 return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop; 807 } 808 } 809