FrictionJoint class
class FrictionJoint extends Joint { final Vector _localAnchorA; final Vector _localAnchorB; Vector _linearImpulse; num _angularImpulse; num _maxForce; num _maxTorque; FrictionJoint(FrictionJointDef def) : super(def), _localAnchorA = new Vector.copy(def.localAnchorA), _localAnchorB = new Vector.copy(def.localAnchorB), _linearImpulse = new Vector(), _angularImpulse = 0.0, _maxForce = def.maxForce, _maxTorque = def.maxTorque { } Vector getLocalAnchorA(Vector argOut) { bodyA.getWorldPointToOut(_localAnchorA, argOut); } Vector getLocalAnchorB(Vector argOut) { bodyB.getWorldPointToOut(_localAnchorB, argOut); } void getReactionForce(num inv_dt, Vector argOut) { argOut.setFrom(_linearImpulse).mulLocal(inv_dt); } num getReactionTorque(num inv_dt) => inv_dt * _angularImpulse; void set maxForce(num force) { assert(force >= 0.0); _maxForce = force; } num get maxForce => _maxForce; void set maxTorque(num torque) { assert(torque >= 0.0); _maxTorque = torque; } num get maxTorque => _maxTorque; void initVelocityConstraints(TimeStep step) { // Compute the effective mass matrix. Vector r1 = new Vector(); Vector r2 = new Vector(); r1.setFrom(_localAnchorA).subLocal(bodyA.localCenter); r2.setFrom(_localAnchorB).subLocal(bodyB.localCenter); Matrix22.mulMatrixAndVectorToOut(bodyA.originTransform.rotation, r1, r1); Matrix22.mulMatrixAndVectorToOut(bodyB.originTransform.rotation, r2, r2); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] Matrix22 K = new Matrix22(); K.col1.x = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.y * r1.y + bodyB.invInertia * r2.y * r2.y; K.col1.y = -bodyA.invInertia * r1.x * r1.y - bodyB.invInertia * r2.x * r2.y; K.col2.x = K.col1.y; K.col2.y = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.x * r1.x + bodyB.invInertia * r2.x * r2.x; Matrix22 linearMass = new Matrix22(); linearMass.setFrom(K); linearMass.invertLocal(); num angularMass = bodyA.invInertia + bodyB.invInertia; if (angularMass > 0.0) { angularMass = 1.0 / angularMass; } if (step.warmStarting) { // Scale impulses. _linearImpulse.mulLocal(step.dtRatio); _angularImpulse *= step.dtRatio; Vector P = new Vector(); P.setFrom(_linearImpulse); bodyA.linearVelocity.x -= bodyA.invMass * P.x; bodyA.linearVelocity.y -= bodyA.invMass * P.y; bodyA.angularVelocity -= bodyA.invInertia * (Vector.crossVectors(r1, P) + _angularImpulse); bodyB.linearVelocity.x += bodyB.invMass * P.x; bodyB.linearVelocity.y += bodyB.invMass * P.y; bodyB.angularVelocity += bodyB.invInertia * (Vector.crossVectors(r2, P) + _angularImpulse); } else { _linearImpulse.setZero(); _angularImpulse = 0.0; } } void solveVelocityConstraints(TimeStep step) { // Solve angular friction { final num Cdot = bodyB.angularVelocity - bodyA.angularVelocity; num angularMass = bodyA.invInertia + bodyB.invInertia; if (angularMass > 0.0) { angularMass = 1.0 / angularMass; } num impulse = -angularMass * Cdot; final num oldImpulse = _angularImpulse; final num maxImpulse = step.dt * _maxTorque; _angularImpulse = MathBox.clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = _angularImpulse - oldImpulse; bodyA.angularVelocity -= bodyA.invInertia * impulse; bodyB.angularVelocity += bodyB.invInertia * impulse; } // Solve linear friction { Vector r1 = new Vector(); Vector r2 = new Vector(); r1.setFrom(_localAnchorA).subLocal(bodyA.localCenter); r2.setFrom(_localAnchorB).subLocal(bodyB.localCenter); Matrix22.mulMatrixAndVectorToOut(bodyA.originTransform.rotation, r1, r1); Matrix22.mulMatrixAndVectorToOut(bodyB.originTransform.rotation, r2, r2); Vector temp = new Vector(); Vector Cdot = new Vector(); Vector.crossNumAndVectorToOut(bodyA.angularVelocity, r1, temp); Vector.crossNumAndVectorToOut(bodyB.angularVelocity, r2, Cdot); Cdot.addLocal(bodyB.linearVelocity).subLocal(bodyA.linearVelocity).subLocal(temp); Matrix22 K = new Matrix22(); K.col1.x = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.y * r1.y + bodyB.invInertia * r2.y * r2.y; K.col1.y = -bodyA.invInertia * r1.x * r1.y - bodyB.invInertia * r2.x * r2.y; K.col2.x = K.col1.y; K.col2.y = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.x * r1.x + bodyB.invInertia * r2.x * r2.x; Matrix22 linearMass = new Matrix22(); linearMass.setFrom(K); linearMass.invertLocal(); Vector impulse = new Vector(); Matrix22.mulMatrixAndVectorToOut(linearMass, Cdot, impulse); impulse.negateLocal(); Vector oldImpulse = new Vector(); oldImpulse.setFrom(_linearImpulse); _linearImpulse.addLocal(impulse); num maxImpulse = step.dt * _maxForce; if (_linearImpulse.lengthSquared > maxImpulse * maxImpulse) { _linearImpulse.normalize(); _linearImpulse.mulLocal(maxImpulse); } impulse.setFrom(_linearImpulse).subLocal(oldImpulse); bodyA.linearVelocity.x -= impulse.x * bodyA.invMass; bodyA.linearVelocity.y -= impulse.y * bodyA.invMass; bodyA.angularVelocity -= bodyA.invInertia * Vector.crossVectors(r1, impulse); bodyB.linearVelocity.x += impulse.x * bodyB.invMass; bodyB.linearVelocity.y += impulse.y * bodyB.invMass; bodyB.angularVelocity += bodyB.invInertia * Vector.crossVectors(r2, impulse); } } bool solvePositionConstraints(num baumgarte) => true; }
Extends
Joint > FrictionJoint
Constructors
new FrictionJoint(FrictionJointDef def) #
FrictionJoint(FrictionJointDef def) : super(def), _localAnchorA = new Vector.copy(def.localAnchorA), _localAnchorB = new Vector.copy(def.localAnchorB), _linearImpulse = new Vector(), _angularImpulse = 0.0, _maxForce = def.maxForce, _maxTorque = def.maxTorque { }
Properties
final bool active #
inherited from Joint
Short-cut function to determine if either body is inactive.
bool get active => bodyA.active && bodyB.active;
num maxForce #
num get maxForce => _maxForce;
void set maxForce(num force) { assert(force >= 0.0); _maxForce = force; }
num maxTorque #
num get maxTorque => _maxTorque;
void set maxTorque(num torque) { assert(torque >= 0.0); _maxTorque = torque; }
Methods
void destructor() #
inherited from Joint
Override to handle destruction of joint.
void destructor() { }
void getAnchorA(Vector argOut) #
inherited from Joint
Get the anchor point on bodyA in world coordinates.
void getAnchorA(Vector argOut) { }
void getAnchorB(Vector argOut) #
inherited from Joint
Get the anchor point on bodyB in world coordinates.
void getAnchorB(Vector argOut) { }
Vector getLocalAnchorA(Vector argOut) #
Vector getLocalAnchorA(Vector argOut) { bodyA.getWorldPointToOut(_localAnchorA, argOut); }
Vector getLocalAnchorB(Vector argOut) #
Vector getLocalAnchorB(Vector argOut) { bodyB.getWorldPointToOut(_localAnchorB, argOut); }
void getReactionForce(num inv_dt, Vector argOut) #
Get the reaction force on body2 at the joint anchor in Newtons.
docs inherited from Joint
void getReactionForce(num inv_dt, Vector argOut) { argOut.setFrom(_linearImpulse).mulLocal(inv_dt); }
num getReactionTorque(num inv_dt) #
Get the reaction torque on body2 in N*m.
docs inherited from Joint
num getReactionTorque(num inv_dt) => inv_dt * _angularImpulse;
void initVelocityConstraints(TimeStep step) #
void initVelocityConstraints(TimeStep step) { // Compute the effective mass matrix. Vector r1 = new Vector(); Vector r2 = new Vector(); r1.setFrom(_localAnchorA).subLocal(bodyA.localCenter); r2.setFrom(_localAnchorB).subLocal(bodyB.localCenter); Matrix22.mulMatrixAndVectorToOut(bodyA.originTransform.rotation, r1, r1); Matrix22.mulMatrixAndVectorToOut(bodyB.originTransform.rotation, r2, r2); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] Matrix22 K = new Matrix22(); K.col1.x = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.y * r1.y + bodyB.invInertia * r2.y * r2.y; K.col1.y = -bodyA.invInertia * r1.x * r1.y - bodyB.invInertia * r2.x * r2.y; K.col2.x = K.col1.y; K.col2.y = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.x * r1.x + bodyB.invInertia * r2.x * r2.x; Matrix22 linearMass = new Matrix22(); linearMass.setFrom(K); linearMass.invertLocal(); num angularMass = bodyA.invInertia + bodyB.invInertia; if (angularMass > 0.0) { angularMass = 1.0 / angularMass; } if (step.warmStarting) { // Scale impulses. _linearImpulse.mulLocal(step.dtRatio); _angularImpulse *= step.dtRatio; Vector P = new Vector(); P.setFrom(_linearImpulse); bodyA.linearVelocity.x -= bodyA.invMass * P.x; bodyA.linearVelocity.y -= bodyA.invMass * P.y; bodyA.angularVelocity -= bodyA.invInertia * (Vector.crossVectors(r1, P) + _angularImpulse); bodyB.linearVelocity.x += bodyB.invMass * P.x; bodyB.linearVelocity.y += bodyB.invMass * P.y; bodyB.angularVelocity += bodyB.invInertia * (Vector.crossVectors(r2, P) + _angularImpulse); } else { _linearImpulse.setZero(); _angularImpulse = 0.0; } }
bool solvePositionConstraints(num baumgarte) #
This returns true if the position errors are within tolerance.
docs inherited from Joint
bool solvePositionConstraints(num baumgarte) => true;
void solveVelocityConstraints(TimeStep step) #
void solveVelocityConstraints(TimeStep step) { // Solve angular friction { final num Cdot = bodyB.angularVelocity - bodyA.angularVelocity; num angularMass = bodyA.invInertia + bodyB.invInertia; if (angularMass > 0.0) { angularMass = 1.0 / angularMass; } num impulse = -angularMass * Cdot; final num oldImpulse = _angularImpulse; final num maxImpulse = step.dt * _maxTorque; _angularImpulse = MathBox.clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = _angularImpulse - oldImpulse; bodyA.angularVelocity -= bodyA.invInertia * impulse; bodyB.angularVelocity += bodyB.invInertia * impulse; } // Solve linear friction { Vector r1 = new Vector(); Vector r2 = new Vector(); r1.setFrom(_localAnchorA).subLocal(bodyA.localCenter); r2.setFrom(_localAnchorB).subLocal(bodyB.localCenter); Matrix22.mulMatrixAndVectorToOut(bodyA.originTransform.rotation, r1, r1); Matrix22.mulMatrixAndVectorToOut(bodyB.originTransform.rotation, r2, r2); Vector temp = new Vector(); Vector Cdot = new Vector(); Vector.crossNumAndVectorToOut(bodyA.angularVelocity, r1, temp); Vector.crossNumAndVectorToOut(bodyB.angularVelocity, r2, Cdot); Cdot.addLocal(bodyB.linearVelocity).subLocal(bodyA.linearVelocity).subLocal(temp); Matrix22 K = new Matrix22(); K.col1.x = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.y * r1.y + bodyB.invInertia * r2.y * r2.y; K.col1.y = -bodyA.invInertia * r1.x * r1.y - bodyB.invInertia * r2.x * r2.y; K.col2.x = K.col1.y; K.col2.y = bodyA.invMass + bodyB.invMass + bodyA.invInertia * r1.x * r1.x + bodyB.invInertia * r2.x * r2.x; Matrix22 linearMass = new Matrix22(); linearMass.setFrom(K); linearMass.invertLocal(); Vector impulse = new Vector(); Matrix22.mulMatrixAndVectorToOut(linearMass, Cdot, impulse); impulse.negateLocal(); Vector oldImpulse = new Vector(); oldImpulse.setFrom(_linearImpulse); _linearImpulse.addLocal(impulse); num maxImpulse = step.dt * _maxForce; if (_linearImpulse.lengthSquared > maxImpulse * maxImpulse) { _linearImpulse.normalize(); _linearImpulse.mulLocal(maxImpulse); } impulse.setFrom(_linearImpulse).subLocal(oldImpulse); bodyA.linearVelocity.x -= impulse.x * bodyA.invMass; bodyA.linearVelocity.y -= impulse.y * bodyA.invMass; bodyA.angularVelocity -= bodyA.invInertia * Vector.crossVectors(r1, impulse); bodyB.linearVelocity.x += impulse.x * bodyB.invMass; bodyB.linearVelocity.y += impulse.y * bodyB.invMass; bodyB.angularVelocity += bodyB.invInertia * Vector.crossVectors(r2, impulse); } }