b2MouseJoint.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Dynamics/Joints/b2MouseJoint.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2TimeStep.h>
00022 
00023 // p = attached point, m = mouse point
00024 // C = p - m
00025 // Cdot = v
00026 //      = v + cross(w, r)
00027 // J = [I r_skew]
00028 // Identity used:
00029 // w k % (rx i + ry j) = w * (-ry i + rx j)
00030 
00031 b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
00032 : b2Joint(def)
00033 {
00034         b2Assert(def->target.IsValid());
00035         b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
00036         b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
00037         b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
00038 
00039         m_targetA = def->target;
00040         m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
00041 
00042         m_maxForce = def->maxForce;
00043         m_impulse.SetZero();
00044 
00045         m_frequencyHz = def->frequencyHz;
00046         m_dampingRatio = def->dampingRatio;
00047 
00048         m_beta = 0.0f;
00049         m_gamma = 0.0f;
00050 }
00051 
00052 void b2MouseJoint::SetTarget(const b2Vec2& target)
00053 {
00054         if (m_bodyB->IsAwake() == false)
00055         {
00056                 m_bodyB->SetAwake(true);
00057         }
00058         m_targetA = target;
00059 }
00060 
00061 const b2Vec2& b2MouseJoint::GetTarget() const
00062 {
00063         return m_targetA;
00064 }
00065 
00066 void b2MouseJoint::SetMaxForce(float32 force)
00067 {
00068         m_maxForce = force;
00069 }
00070 
00071 float32 b2MouseJoint::GetMaxForce() const
00072 {
00073         return m_maxForce;
00074 }
00075 
00076 void b2MouseJoint::SetFrequency(float32 hz)
00077 {
00078         m_frequencyHz = hz;
00079 }
00080 
00081 float32 b2MouseJoint::GetFrequency() const
00082 {
00083         return m_frequencyHz;
00084 }
00085 
00086 void b2MouseJoint::SetDampingRatio(float32 ratio)
00087 {
00088         m_dampingRatio = ratio;
00089 }
00090 
00091 float32 b2MouseJoint::GetDampingRatio() const
00092 {
00093         return m_dampingRatio;
00094 }
00095 
00096 void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
00097 {
00098         m_indexB = m_bodyB->m_islandIndex;
00099         m_localCenterB = m_bodyB->m_sweep.localCenter;
00100         m_invMassB = m_bodyB->m_invMass;
00101         m_invIB = m_bodyB->m_invI;
00102 
00103         b2Vec2 cB = data.positions[m_indexB].c;
00104         float32 aB = data.positions[m_indexB].a;
00105         b2Vec2 vB = data.velocities[m_indexB].v;
00106         float32 wB = data.velocities[m_indexB].w;
00107 
00108         b2Rot qB(aB);
00109 
00110         float32 mass = m_bodyB->GetMass();
00111 
00112         // Frequency
00113         float32 omega = 2.0f * b2_pi * m_frequencyHz;
00114 
00115         // Damping coefficient
00116         float32 d = 2.0f * mass * m_dampingRatio * omega;
00117 
00118         // Spring stiffness
00119         float32 k = mass * (omega * omega);
00120 
00121         // magic formulas
00122         // gamma has units of inverse mass.
00123         // beta has units of inverse time.
00124         float32 h = data.step.dt;
00125         b2Assert(d + h * k > b2_epsilon);
00126         m_gamma = h * (d + h * k);
00127         if (m_gamma != 0.0f)
00128         {
00129                 m_gamma = 1.0f / m_gamma;
00130         }
00131         m_beta = h * k * m_gamma;
00132 
00133         // Compute the effective mass matrix.
00134         m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
00135 
00136         // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
00137         //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
00138         //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
00139         b2Mat22 K;
00140         K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
00141         K.ex.y = -m_invIB * m_rB.x * m_rB.y;
00142         K.ey.x = K.ex.y;
00143         K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
00144 
00145         m_mass = K.GetInverse();
00146 
00147         m_C = cB + m_rB - m_targetA;
00148         m_C *= m_beta;
00149 
00150         // Cheat with some damping
00151         wB *= 0.98f;
00152 
00153         if (data.step.warmStarting)
00154         {
00155                 m_impulse *= data.step.dtRatio;
00156                 vB += m_invMassB * m_impulse;
00157                 wB += m_invIB * b2Cross(m_rB, m_impulse);
00158         }
00159         else
00160         {
00161                 m_impulse.SetZero();
00162         }
00163 
00164         data.velocities[m_indexB].v = vB;
00165         data.velocities[m_indexB].w = wB;
00166 }
00167 
00168 void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
00169 {
00170         b2Vec2 vB = data.velocities[m_indexB].v;
00171         float32 wB = data.velocities[m_indexB].w;
00172 
00173         // Cdot = v + cross(w, r)
00174         b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
00175         b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
00176 
00177         b2Vec2 oldImpulse = m_impulse;
00178         m_impulse += impulse;
00179         float32 maxImpulse = data.step.dt * m_maxForce;
00180         if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
00181         {
00182                 m_impulse *= maxImpulse / m_impulse.Length();
00183         }
00184         impulse = m_impulse - oldImpulse;
00185 
00186         vB += m_invMassB * impulse;
00187         wB += m_invIB * b2Cross(m_rB, impulse);
00188 
00189         data.velocities[m_indexB].v = vB;
00190         data.velocities[m_indexB].w = wB;
00191 }
00192 
00193 bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
00194 {
00195         B2_NOT_USED(data);
00196         return true;
00197 }
00198 
00199 b2Vec2 b2MouseJoint::GetAnchorA() const
00200 {
00201         return m_targetA;
00202 }
00203 
00204 b2Vec2 b2MouseJoint::GetAnchorB() const
00205 {
00206         return m_bodyB->GetWorldPoint(m_localAnchorB);
00207 }
00208 
00209 b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
00210 {
00211         return inv_dt * m_impulse;
00212 }
00213 
00214 float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
00215 {
00216         return inv_dt * 0.0f;
00217 }
00218 
00219 void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
00220 {
00221         m_targetA -= newOrigin;
00222 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:47