1 /*
2 * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
3 *
4 * This software is provided 'as-is', without any express or implied
5 * warranty. In no event will the authors be held liable for any damages
6 * arising from the use of this software.
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 * 1. The origin of this software must not be misrepresented; you must not
11 * claim that you wrote the original software. If you use this software
12 * in a product, an acknowledgment in the product documentation would be
13 * appreciated but is not required.
14 * 2. Altered source versions must be plainly marked as such, and must not be
15 * misrepresented as being the original software.
16 * 3. This notice may not be removed or altered from any source distribution.
17 */
18
19 #include "b2FrictionJoint.h"
20 #include "b2Body.h"
21 #include "b2TimeStep.h"
22
23 // Point-to-point constraint
24 // Cdot = v2 - v1
25 // = v2 + cross(w2, r2) - v1 - cross(w1, r1)
26 // J = [-I -r1_skew I r2_skew ]
27 // Identity used:
28 // w k % (rx i + ry j) = w * (-ry i + rx j)
29
30 // Angle constraint
31 // Cdot = w2 - w1
32 // J = [0 0 -1 0 0 1]
33 // K = invI1 + invI2
34
Initialize(b2Body * bA,b2Body * bB,const b2Vec2 & anchor)35 void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
36 {
37 bodyA = bA;
38 bodyB = bB;
39 localAnchorA = bodyA->GetLocalPoint(anchor);
40 localAnchorB = bodyB->GetLocalPoint(anchor);
41 }
42
b2FrictionJoint(const b2FrictionJointDef * def)43 b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
44 : b2Joint(def)
45 {
46 m_localAnchorA = def->localAnchorA;
47 m_localAnchorB = def->localAnchorB;
48
49 m_linearImpulse.SetZero();
50 m_angularImpulse = 0.0f;
51
52 m_maxForce = def->maxForce;
53 m_maxTorque = def->maxTorque;
54 }
55
InitVelocityConstraints(const b2TimeStep & step)56 void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
57 {
58 b2Body* bA = m_bodyA;
59 b2Body* bB = m_bodyB;
60
61 // Compute the effective mass matrix.
62 b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
63 b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
64
65 // J = [-I -r1_skew I r2_skew]
66 // [ 0 -1 0 1]
67 // r_skew = [-ry; rx]
68
69 // Matlab
70 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
71 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
72 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
73
74 float32 mA = bA->m_invMass, mB = bB->m_invMass;
75 float32 iA = bA->m_invI, iB = bB->m_invI;
76
77 b2Mat22 K1;
78 K1.col1.x = mA + mB; K1.col2.x = 0.0f;
79 K1.col1.y = 0.0f; K1.col2.y = mA + mB;
80
81 b2Mat22 K2;
82 K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
83 K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x;
84
85 b2Mat22 K3;
86 K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
87 K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x;
88
89 b2Mat22 K = K1 + K2 + K3;
90 m_linearMass = K.GetInverse();
91
92 m_angularMass = iA + iB;
93 if (m_angularMass > 0.0f)
94 {
95 m_angularMass = 1.0f / m_angularMass;
96 }
97
98 if (step.warmStarting)
99 {
100 // Scale impulses to support a variable time step.
101 m_linearImpulse *= step.dtRatio;
102 m_angularImpulse *= step.dtRatio;
103
104 b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
105
106 bA->m_linearVelocity -= mA * P;
107 bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
108
109 bB->m_linearVelocity += mB * P;
110 bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse);
111 }
112 else
113 {
114 m_linearImpulse.SetZero();
115 m_angularImpulse = 0.0f;
116 }
117 }
118
SolveVelocityConstraints(const b2TimeStep & step)119 void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
120 {
121 B2_NOT_USED(step);
122
123 b2Body* bA = m_bodyA;
124 b2Body* bB = m_bodyB;
125
126 b2Vec2 vA = bA->m_linearVelocity;
127 float32 wA = bA->m_angularVelocity;
128 b2Vec2 vB = bB->m_linearVelocity;
129 float32 wB = bB->m_angularVelocity;
130
131 float32 mA = bA->m_invMass, mB = bB->m_invMass;
132 float32 iA = bA->m_invI, iB = bB->m_invI;
133
134 b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
135 b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
136
137 // Solve angular friction
138 {
139 float32 Cdot = wB - wA;
140 float32 impulse = -m_angularMass * Cdot;
141
142 float32 oldImpulse = m_angularImpulse;
143 float32 maxImpulse = step.dt * m_maxTorque;
144 m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
145 impulse = m_angularImpulse - oldImpulse;
146
147 wA -= iA * impulse;
148 wB += iB * impulse;
149 }
150
151 // Solve linear friction
152 {
153 b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
154
155 b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
156 b2Vec2 oldImpulse = m_linearImpulse;
157 m_linearImpulse += impulse;
158
159 float32 maxImpulse = step.dt * m_maxForce;
160
161 if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
162 {
163 m_linearImpulse.Normalize();
164 m_linearImpulse *= maxImpulse;
165 }
166
167 impulse = m_linearImpulse - oldImpulse;
168
169 vA -= mA * impulse;
170 wA -= iA * b2Cross(rA, impulse);
171
172 vB += mB * impulse;
173 wB += iB * b2Cross(rB, impulse);
174 }
175
176 bA->m_linearVelocity = vA;
177 bA->m_angularVelocity = wA;
178 bB->m_linearVelocity = vB;
179 bB->m_angularVelocity = wB;
180 }
181
SolvePositionConstraints(float32 baumgarte)182 bool b2FrictionJoint::SolvePositionConstraints(float32 baumgarte)
183 {
184 B2_NOT_USED(baumgarte);
185
186 return true;
187 }
188
GetAnchorA() const189 b2Vec2 b2FrictionJoint::GetAnchorA() const
190 {
191 return m_bodyA->GetWorldPoint(m_localAnchorA);
192 }
193
GetAnchorB() const194 b2Vec2 b2FrictionJoint::GetAnchorB() const
195 {
196 return m_bodyB->GetWorldPoint(m_localAnchorB);
197 }
198
GetReactionForce(float32 inv_dt) const199 b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
200 {
201 return inv_dt * m_linearImpulse;
202 }
203
GetReactionTorque(float32 inv_dt) const204 float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
205 {
206 return inv_dt * m_angularImpulse;
207 }
208
SetMaxForce(float32 force)209 void b2FrictionJoint::SetMaxForce(float32 force)
210 {
211 b2Assert(b2IsValid(force) && force >= 0.0f);
212 m_maxForce = force;
213 }
214
GetMaxForce() const215 float32 b2FrictionJoint::GetMaxForce() const
216 {
217 return m_maxForce;
218 }
219
SetMaxTorque(float32 torque)220 void b2FrictionJoint::SetMaxTorque(float32 torque)
221 {
222 b2Assert(b2IsValid(torque) && torque >= 0.0f);
223 m_maxTorque = torque;
224 }
225
GetMaxTorque() const226 float32 b2FrictionJoint::GetMaxTorque() const
227 {
228 return m_maxTorque;
229 }
230