← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp
 
1
/*
 
2
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
 
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 <Box2D/Dynamics/Joints/b2MouseJoint.h>
 
20
#include <Box2D/Dynamics/b2Body.h>
 
21
#include <Box2D/Dynamics/b2TimeStep.h>
 
22
 
 
23
// p = attached point, m = mouse point
 
24
// C = p - m
 
25
// Cdot = v
 
26
//      = v + cross(w, r)
 
27
// J = [I r_skew]
 
28
// Identity used:
 
29
// w k % (rx i + ry j) = w * (-ry i + rx j)
 
30
 
 
31
b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
 
32
: b2Joint(def)
 
33
{
 
34
        b2Assert(def->target.IsValid());
 
35
        b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
 
36
        b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
 
37
        b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
 
38
 
 
39
        m_targetA = def->target;
 
40
        m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
 
41
 
 
42
        m_maxForce = def->maxForce;
 
43
        m_impulse.SetZero();
 
44
 
 
45
        m_frequencyHz = def->frequencyHz;
 
46
        m_dampingRatio = def->dampingRatio;
 
47
 
 
48
        m_beta = 0.0f;
 
49
        m_gamma = 0.0f;
 
50
}
 
51
 
 
52
void b2MouseJoint::SetTarget(const b2Vec2& target)
 
53
{
 
54
        if (m_bodyB->IsAwake() == false)
 
55
        {
 
56
                m_bodyB->SetAwake(true);
 
57
        }
 
58
        m_targetA = target;
 
59
}
 
60
 
 
61
const b2Vec2& b2MouseJoint::GetTarget() const
 
62
{
 
63
        return m_targetA;
 
64
}
 
65
 
 
66
void b2MouseJoint::SetMaxForce(float32 force)
 
67
{
 
68
        m_maxForce = force;
 
69
}
 
70
 
 
71
float32 b2MouseJoint::GetMaxForce() const
 
72
{
 
73
        return m_maxForce;
 
74
}
 
75
 
 
76
void b2MouseJoint::SetFrequency(float32 hz)
 
77
{
 
78
        m_frequencyHz = hz;
 
79
}
 
80
 
 
81
float32 b2MouseJoint::GetFrequency() const
 
82
{
 
83
        return m_frequencyHz;
 
84
}
 
85
 
 
86
void b2MouseJoint::SetDampingRatio(float32 ratio)
 
87
{
 
88
        m_dampingRatio = ratio;
 
89
}
 
90
 
 
91
float32 b2MouseJoint::GetDampingRatio() const
 
92
{
 
93
        return m_dampingRatio;
 
94
}
 
95
 
 
96
void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
 
97
{
 
98
        m_indexB = m_bodyB->m_islandIndex;
 
99
        m_localCenterB = m_bodyB->m_sweep.localCenter;
 
100
        m_invMassB = m_bodyB->m_invMass;
 
101
        m_invIB = m_bodyB->m_invI;
 
102
 
 
103
        b2Vec2 cB = data.positions[m_indexB].c;
 
104
        float32 aB = data.positions[m_indexB].a;
 
105
        b2Vec2 vB = data.velocities[m_indexB].v;
 
106
        float32 wB = data.velocities[m_indexB].w;
 
107
 
 
108
        b2Rot qB(aB);
 
109
 
 
110
        float32 mass = m_bodyB->GetMass();
 
111
 
 
112
        // Frequency
 
113
        float32 omega = 2.0f * b2_pi * m_frequencyHz;
 
114
 
 
115
        // Damping coefficient
 
116
        float32 d = 2.0f * mass * m_dampingRatio * omega;
 
117
 
 
118
        // Spring stiffness
 
119
        float32 k = mass * (omega * omega);
 
120
 
 
121
        // magic formulas
 
122
        // gamma has units of inverse mass.
 
123
        // beta has units of inverse time.
 
124
        float32 h = data.step.dt;
 
125
        b2Assert(d + h * k > b2_epsilon);
 
126
        m_gamma = h * (d + h * k);
 
127
        if (m_gamma != 0.0f)
 
128
        {
 
129
                m_gamma = 1.0f / m_gamma;
 
130
        }
 
131
        m_beta = h * k * m_gamma;
 
132
 
 
133
        // Compute the effective mass matrix.
 
134
        m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
135
 
 
136
        // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
 
137
        //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
 
138
        //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
 
139
        b2Mat22 K;
 
140
        K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
 
141
        K.ex.y = -m_invIB * m_rB.x * m_rB.y;
 
142
        K.ey.x = K.ex.y;
 
143
        K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
 
144
 
 
145
        m_mass = K.GetInverse();
 
146
 
 
147
        m_C = cB + m_rB - m_targetA;
 
148
        m_C *= m_beta;
 
149
 
 
150
        // Cheat with some damping
 
151
        wB *= 0.98f;
 
152
 
 
153
        if (data.step.warmStarting)
 
154
        {
 
155
                m_impulse *= data.step.dtRatio;
 
156
                vB += m_invMassB * m_impulse;
 
157
                wB += m_invIB * b2Cross(m_rB, m_impulse);
 
158
        }
 
159
        else
 
160
        {
 
161
                m_impulse.SetZero();
 
162
        }
 
163
 
 
164
        data.velocities[m_indexB].v = vB;
 
165
        data.velocities[m_indexB].w = wB;
 
166
}
 
167
 
 
168
void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
 
169
{
 
170
        b2Vec2 vB = data.velocities[m_indexB].v;
 
171
        float32 wB = data.velocities[m_indexB].w;
 
172
 
 
173
        // Cdot = v + cross(w, r)
 
174
        b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
 
175
        b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
 
176
 
 
177
        b2Vec2 oldImpulse = m_impulse;
 
178
        m_impulse += impulse;
 
179
        float32 maxImpulse = data.step.dt * m_maxForce;
 
180
        if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
 
181
        {
 
182
                m_impulse *= maxImpulse / m_impulse.Length();
 
183
        }
 
184
        impulse = m_impulse - oldImpulse;
 
185
 
 
186
        vB += m_invMassB * impulse;
 
187
        wB += m_invIB * b2Cross(m_rB, impulse);
 
188
 
 
189
        data.velocities[m_indexB].v = vB;
 
190
        data.velocities[m_indexB].w = wB;
 
191
}
 
192
 
 
193
bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
 
194
{
 
195
        B2_NOT_USED(data);
 
196
        return true;
 
197
}
 
198
 
 
199
b2Vec2 b2MouseJoint::GetAnchorA() const
 
200
{
 
201
        return m_targetA;
 
202
}
 
203
 
 
204
b2Vec2 b2MouseJoint::GetAnchorB() const
 
205
{
 
206
        return m_bodyB->GetWorldPoint(m_localAnchorB);
 
207
}
 
208
 
 
209
b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
 
210
{
 
211
        return inv_dt * m_impulse;
 
212
}
 
213
 
 
214
float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
 
215
{
 
216
        return inv_dt * 0.0f;
 
217
}