1 package org.jbox2d.dynamics.joints; 2 3 import org.jbox2d.common.Mat22; 4 import org.jbox2d.common.MathUtils; 5 import org.jbox2d.common.Rot; 6 import org.jbox2d.common.Vec2; 7 import org.jbox2d.dynamics.SolverData; 8 import org.jbox2d.pooling.IWorldPool; 9 10 //Point-to-point constraint 11 //Cdot = v2 - v1 12 // = v2 + cross(w2, r2) - v1 - cross(w1, r1) 13 //J = [-I -r1_skew I r2_skew ] 14 //Identity used: 15 //w k % (rx i + ry j) = w * (-ry i + rx j) 16 17 //Angle constraint 18 //Cdot = w2 - w1 19 //J = [0 0 -1 0 0 1] 20 //K = invI1 + invI2 21 22 /** 23 * A motor joint is used to control the relative motion between two bodies. A typical usage is to 24 * control the movement of a dynamic body with respect to the ground. 25 * 26 * @author dmurph 27 */ 28 public class MotorJoint extends Joint { 29 30 // Solver shared 31 private final Vec2 m_linearOffset = new Vec2(); 32 private float m_angularOffset; 33 private final Vec2 m_linearImpulse = new Vec2(); 34 private float m_angularImpulse; 35 private float m_maxForce; 36 private float m_maxTorque; 37 private float m_correctionFactor; 38 39 // Solver temp 40 private int m_indexA; 41 private int m_indexB; 42 private final Vec2 m_rA = new Vec2(); 43 private final Vec2 m_rB = new Vec2(); 44 private final Vec2 m_localCenterA = new Vec2(); 45 private final Vec2 m_localCenterB = new Vec2(); 46 private final Vec2 m_linearError = new Vec2(); 47 private float m_angularError; 48 private float m_invMassA; 49 private float m_invMassB; 50 private float m_invIA; 51 private float m_invIB; 52 private final Mat22 m_linearMass = new Mat22(); 53 private float m_angularMass; 54 55 public MotorJoint(IWorldPool pool, MotorJointDef def) { 56 super(pool, def); 57 m_linearOffset.set(def.linearOffset); 58 m_angularOffset = def.angularOffset; 59 60 m_angularImpulse = 0.0f; 61 62 m_maxForce = def.maxForce; 63 m_maxTorque = def.maxTorque; 64 m_correctionFactor = def.correctionFactor; 65 } 66 67 @Override 68 public void getAnchorA(Vec2 out) { 69 out.set(m_bodyA.getPosition()); 70 } 71 72 @Override 73 public void getAnchorB(Vec2 out) { 74 out.set(m_bodyB.getPosition()); 75 } 76 77 public void getReactionForce(float inv_dt, Vec2 out) { 78 out.set(m_linearImpulse).mulLocal(inv_dt); 79 } 80 81 public float getReactionTorque(float inv_dt) { 82 return m_angularImpulse * inv_dt; 83 } 84 85 public float getCorrectionFactor() { 86 return m_correctionFactor; 87 } 88 89 public void setCorrectionFactor(float correctionFactor) { 90 this.m_correctionFactor = correctionFactor; 91 } 92 93 /** 94 * Set the target linear offset, in frame A, in meters. 95 */ 96 public void setLinearOffset(Vec2 linearOffset) { 97 if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) { 98 m_bodyA.setAwake(true); 99 m_bodyB.setAwake(true); 100 m_linearOffset.set(linearOffset); 101 } 102 } 103 104 /** 105 * Get the target linear offset, in frame A, in meters. 106 */ 107 public void getLinearOffset(Vec2 out) { 108 out.set(m_linearOffset); 109 } 110 111 /** 112 * Get the target linear offset, in frame A, in meters. Do not modify. 113 */ 114 public Vec2 getLinearOffset() { 115 return m_linearOffset; 116 } 117 118 /** 119 * Set the target angular offset, in radians. 120 * 121 * @param angularOffset 122 */ 123 public void setAngularOffset(float angularOffset) { 124 if (angularOffset != m_angularOffset) { 125 m_bodyA.setAwake(true); 126 m_bodyB.setAwake(true); 127 m_angularOffset = angularOffset; 128 } 129 } 130 131 public float getAngularOffset() { 132 return m_angularOffset; 133 } 134 135 /** 136 * Set the maximum friction force in N. 137 * 138 * @param force 139 */ 140 public void setMaxForce(float force) { 141 assert (force >= 0.0f); 142 m_maxForce = force; 143 } 144 145 /** 146 * Get the maximum friction force in N. 147 */ 148 public float getMaxForce() { 149 return m_maxForce; 150 } 151 152 /** 153 * Set the maximum friction torque in N*m. 154 */ 155 public void setMaxTorque(float torque) { 156 assert (torque >= 0.0f); 157 m_maxTorque = torque; 158 } 159 160 /** 161 * Get the maximum friction torque in N*m. 162 */ 163 public float getMaxTorque() { 164 return m_maxTorque; 165 } 166 167 @Override 168 public void initVelocityConstraints(SolverData data) { 169 m_indexA = m_bodyA.m_islandIndex; 170 m_indexB = m_bodyB.m_islandIndex; 171 m_localCenterA.set(m_bodyA.m_sweep.localCenter); 172 m_localCenterB.set(m_bodyB.m_sweep.localCenter); 173 m_invMassA = m_bodyA.m_invMass; 174 m_invMassB = m_bodyB.m_invMass; 175 m_invIA = m_bodyA.m_invI; 176 m_invIB = m_bodyB.m_invI; 177 178 final Vec2 cA = data.positions[m_indexA].c; 179 float aA = data.positions[m_indexA].a; 180 final Vec2 vA = data.velocities[m_indexA].v; 181 float wA = data.velocities[m_indexA].w; 182 183 final Vec2 cB = data.positions[m_indexB].c; 184 float aB = data.positions[m_indexB].a; 185 final Vec2 vB = data.velocities[m_indexB].v; 186 float wB = data.velocities[m_indexB].w; 187 188 final Rot qA = pool.popRot(); 189 final Rot qB = pool.popRot(); 190 final Vec2 temp = pool.popVec2(); 191 Mat22 K = pool.popMat22(); 192 193 qA.set(aA); 194 qB.set(aB); 195 196 // Compute the effective mass matrix. 197 // m_rA = b2Mul(qA, -m_localCenterA); 198 // m_rB = b2Mul(qB, -m_localCenterB); 199 m_rA.x = qA.c * -m_localCenterA.x - qA.s * -m_localCenterA.y; 200 m_rA.y = qA.s * -m_localCenterA.x + qA.c * -m_localCenterA.y; 201 m_rB.x = qB.c * -m_localCenterB.x - qB.s * -m_localCenterB.y; 202 m_rB.y = qB.s * -m_localCenterB.x + qB.c * -m_localCenterB.y; 203 204 // J = [-I -r1_skew I r2_skew] 205 // [ 0 -1 0 1] 206 // r_skew = [-ry; rx] 207 208 // Matlab 209 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] 210 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] 211 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] 212 float mA = m_invMassA, mB = m_invMassB; 213 float iA = m_invIA, iB = m_invIB; 214 215 K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; 216 K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; 217 K.ey.x = K.ex.y; 218 K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; 219 220 K.invertToOut(m_linearMass); 221 222 m_angularMass = iA + iB; 223 if (m_angularMass > 0.0f) { 224 m_angularMass = 1.0f / m_angularMass; 225 } 226 227 // m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); 228 Rot.mulToOutUnsafe(qA, m_linearOffset, temp); 229 m_linearError.x = cB.x + m_rB.x - cA.x - m_rA.x - temp.x; 230 m_linearError.y = cB.y + m_rB.y - cA.y - m_rA.y - temp.y; 231 m_angularError = aB - aA - m_angularOffset; 232 233 if (data.step.warmStarting) { 234 // Scale impulses to support a variable time step. 235 m_linearImpulse.x *= data.step.dtRatio; 236 m_linearImpulse.y *= data.step.dtRatio; 237 m_angularImpulse *= data.step.dtRatio; 238 239 final Vec2 P = m_linearImpulse; 240 vA.x -= mA * P.x; 241 vA.y -= mA * P.y; 242 wA -= iA * (m_rA.x * P.y - m_rA.y * P.x + m_angularImpulse); 243 vB.x += mB * P.x; 244 vB.y += mB * P.y; 245 wB += iB * (m_rB.x * P.y - m_rB.y * P.x + m_angularImpulse); 246 } else { 247 m_linearImpulse.setZero(); 248 m_angularImpulse = 0.0f; 249 } 250 251 pool.pushVec2(1); 252 pool.pushMat22(1); 253 pool.pushRot(2); 254 255 // data.velocities[m_indexA].v = vA; 256 data.velocities[m_indexA].w = wA; 257 // data.velocities[m_indexB].v = vB; 258 data.velocities[m_indexB].w = wB; 259 } 260 261 @Override 262 public void solveVelocityConstraints(SolverData data) { 263 final Vec2 vA = data.velocities[m_indexA].v; 264 float wA = data.velocities[m_indexA].w; 265 final Vec2 vB = data.velocities[m_indexB].v; 266 float wB = data.velocities[m_indexB].w; 267 268 float mA = m_invMassA, mB = m_invMassB; 269 float iA = m_invIA, iB = m_invIB; 270 271 float h = data.step.dt; 272 float inv_h = data.step.inv_dt; 273 274 final Vec2 temp = pool.popVec2(); 275 276 // Solve angular friction 277 { 278 float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; 279 float impulse = -m_angularMass * Cdot; 280 281 float oldImpulse = m_angularImpulse; 282 float maxImpulse = h * m_maxTorque; 283 m_angularImpulse = MathUtils.clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); 284 impulse = m_angularImpulse - oldImpulse; 285 286 wA -= iA * impulse; 287 wB += iB * impulse; 288 } 289 290 final Vec2 Cdot = pool.popVec2(); 291 292 // Solve linear friction 293 { 294 // Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * 295 // m_linearError; 296 Cdot.x = 297 vB.x + -wB * m_rB.y - vA.x - -wA * m_rA.y + inv_h * m_correctionFactor * m_linearError.x; 298 Cdot.y = 299 vB.y + wB * m_rB.x - vA.y - wA * m_rA.x + inv_h * m_correctionFactor * m_linearError.y; 300 301 final Vec2 impulse = temp; 302 Mat22.mulToOutUnsafe(m_linearMass, Cdot, impulse); 303 impulse.negateLocal(); 304 final Vec2 oldImpulse = pool.popVec2(); 305 oldImpulse.set(m_linearImpulse); 306 m_linearImpulse.addLocal(impulse); 307 308 float maxImpulse = h * m_maxForce; 309 310 if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { 311 m_linearImpulse.normalize(); 312 m_linearImpulse.mulLocal(maxImpulse); 313 } 314 315 impulse.x = m_linearImpulse.x - oldImpulse.x; 316 impulse.y = m_linearImpulse.y - oldImpulse.y; 317 318 vA.x -= mA * impulse.x; 319 vA.y -= mA * impulse.y; 320 wA -= iA * (m_rA.x * impulse.y - m_rA.y * impulse.x); 321 322 vB.x += mB * impulse.x; 323 vB.y += mB * impulse.y; 324 wB += iB * (m_rB.x * impulse.y - m_rB.y * impulse.x); 325 } 326 327 pool.pushVec2(3); 328 329 // data.velocities[m_indexA].v.set(vA); 330 data.velocities[m_indexA].w = wA; 331 // data.velocities[m_indexB].v.set(vB); 332 data.velocities[m_indexB].w = wB; 333 } 334 335 @Override 336 public boolean solvePositionConstraints(SolverData data) { 337 return true; 338 } 339 } 340