git.maemo.org Git - blok/blob - Box2D/Source/Dynamics/Joints/b2GearJoint.cpp

   1 /*
   2 * Copyright (c) 2007 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 "b2GearJoint.h"
  20 #include "b2RevoluteJoint.h"
  21 #include "b2PrismaticJoint.h"
  22 #include "../b2Body.h"
  23 #include "../b2World.h"
  24
  25 // Gear Joint:
  26 // C0 = (coordinate1 + ratio * coordinate2)_initial
  27 // C = C0 - (cordinate1 + ratio * coordinate2) = 0
  28 // Cdot = -(Cdot1 + ratio * Cdot2)
  29 // J = -[J1 ratio * J2]
  30 // K = J * invM * JT
  31 //   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
  32 //
  33 // Revolute:
  34 // coordinate = rotation
  35 // Cdot = angularVelocity
  36 // J = [0 0 1]
  37 // K = J * invM * JT = invI
  38 //
  39 // Prismatic:
  40 // coordinate = dot(p - pg, ug)
  41 // Cdot = dot(v + cross(w, r), ug)
  42 // J = [ug cross(r, ug)]
  43 // K = J * invM * JT = invMass + invI * cross(r, ug)^2
  44
  45 b2GearJoint::b2GearJoint(const b2GearJointDef* def)
  46 : b2Joint(def)
  47 {
  48         b2JointType type1 = def->joint1->GetType();
  49         b2JointType type2 = def->joint2->GetType();
  50
  51         b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
  52         b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
  53         b2Assert(def->joint1->GetBody1()->IsStatic());
  54         b2Assert(def->joint2->GetBody1()->IsStatic());
  55
  56         m_revolute1 = NULL;
  57         m_prismatic1 = NULL;
  58         m_revolute2 = NULL;
  59         m_prismatic2 = NULL;
  60
  61         float32 coordinate1, coordinate2;
  62
  63         m_ground1 = def->joint1->GetBody1();
  64         m_body1 = def->joint1->GetBody2();
  65         if (type1 == e_revoluteJoint)
  66         {
  67                 m_revolute1 = (b2RevoluteJoint*)def->joint1;
  68                 m_groundAnchor1 = m_revolute1->m_localAnchor1;
  69                 m_localAnchor1 = m_revolute1->m_localAnchor2;
  70                 coordinate1 = m_revolute1->GetJointAngle();
  71         }
  72         else
  73         {
  74                 m_prismatic1 = (b2PrismaticJoint*)def->joint1;
  75                 m_groundAnchor1 = m_prismatic1->m_localAnchor1;
  76                 m_localAnchor1 = m_prismatic1->m_localAnchor2;
  77                 coordinate1 = m_prismatic1->GetJointTranslation();
  78         }
  79
  80         m_ground2 = def->joint2->GetBody1();
  81         m_body2 = def->joint2->GetBody2();
  82         if (type2 == e_revoluteJoint)
  83         {
  84                 m_revolute2 = (b2RevoluteJoint*)def->joint2;
  85                 m_groundAnchor2 = m_revolute2->m_localAnchor1;
  86                 m_localAnchor2 = m_revolute2->m_localAnchor2;
  87                 coordinate2 = m_revolute2->GetJointAngle();
  88         }
  89         else
  90         {
  91                 m_prismatic2 = (b2PrismaticJoint*)def->joint2;
  92                 m_groundAnchor2 = m_prismatic2->m_localAnchor1;
  93                 m_localAnchor2 = m_prismatic2->m_localAnchor2;
  94                 coordinate2 = m_prismatic2->GetJointTranslation();
  95         }
  96
  97         m_ratio = def->ratio;
  98
  99         m_constant = coordinate1 + m_ratio * coordinate2;
 100
 101         m_force = 0.0f;
 102 }
 103
 104 void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
 105 {
 106         b2Body* g1 = m_ground1;
 107         b2Body* g2 = m_ground2;
 108         b2Body* b1 = m_body1;
 109         b2Body* b2 = m_body2;
 110
 111         float32 K = 0.0f;
 112         m_J.SetZero();
 113
 114         if (m_revolute1)
 115         {
 116                 m_J.angular1 = -1.0f;
 117                 K += b1->m_invI;
 118         }
 119         else
 120         {
 121                 b2Vec2 ug = b2Mul(g1->GetXForm().R, m_prismatic1->m_localXAxis1);
 122                 b2Vec2 r = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
 123                 float32 crug = b2Cross(r, ug);
 124                 m_J.linear1 = -ug;
 125                 m_J.angular1 = -crug;
 126                 K += b1->m_invMass + b1->m_invI * crug * crug;
 127         }
 128
 129         if (m_revolute2)
 130         {
 131                 m_J.angular2 = -m_ratio;
 132                 K += m_ratio * m_ratio * b2->m_invI;
 133         }
 134         else
 135         {
 136                 b2Vec2 ug = b2Mul(g2->GetXForm().R, m_prismatic2->m_localXAxis1);
 137                 b2Vec2 r = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
 138                 float32 crug = b2Cross(r, ug);
 139                 m_J.linear2 = -m_ratio * ug;
 140                 m_J.angular2 = -m_ratio * crug;
 141                 K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
 142         }
 143
 144         // Compute effective mass.
 145         b2Assert(K > 0.0f);
 146         m_mass = 1.0f / K;
 147
 148         if (step.warmStarting)
 149         {
 150                 // Warm starting.
 151                 float32 P = B2FORCE_SCALE(step.dt) * m_force;
 152                 b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
 153                 b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
 154                 b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
 155                 b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
 156         }
 157         else
 158         {
 159                 m_force = 0.0f;
 160         }
 161 }
 162
 163 void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
 164 {
 165         b2Body* b1 = m_body1;
 166         b2Body* b2 = m_body2;
 167
 168         float32 Cdot = m_J.Compute(     b1->m_linearVelocity, b1->m_angularVelocity,
 169                                                                 b2->m_linearVelocity, b2->m_angularVelocity);
 170
 171         float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_mass * Cdot;
 172         m_force += force;
 173
 174         float32 P = B2FORCE_SCALE(step.dt) * force;
 175         b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
 176         b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
 177         b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
 178         b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
 179 }
 180
 181 bool b2GearJoint::SolvePositionConstraints()
 182 {
 183         float32 linearError = 0.0f;
 184
 185         b2Body* b1 = m_body1;
 186         b2Body* b2 = m_body2;
 187
 188         float32 coordinate1, coordinate2;
 189         if (m_revolute1)
 190         {
 191                 coordinate1 = m_revolute1->GetJointAngle();
 192         }
 193         else
 194         {
 195                 coordinate1 = m_prismatic1->GetJointTranslation();
 196         }
 197
 198         if (m_revolute2)
 199         {
 200                 coordinate2 = m_revolute2->GetJointAngle();
 201         }
 202         else
 203         {
 204                 coordinate2 = m_prismatic2->GetJointTranslation();
 205         }
 206
 207         float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
 208
 209         float32 impulse = -m_mass * C;
 210
 211         b1->m_sweep.c += b1->m_invMass * impulse * m_J.linear1;
 212         b1->m_sweep.a += b1->m_invI * impulse * m_J.angular1;
 213         b2->m_sweep.c += b2->m_invMass * impulse * m_J.linear2;
 214         b2->m_sweep.a += b2->m_invI * impulse * m_J.angular2;
 215
 216         b1->SynchronizeTransform();
 217         b2->SynchronizeTransform();
 218
 219         return linearError < b2_linearSlop;
 220 }
 221
 222 b2Vec2 b2GearJoint::GetAnchor1() const
 223 {
 224         return m_body1->GetWorldPoint(m_localAnchor1);
 225 }
 226
 227 b2Vec2 b2GearJoint::GetAnchor2() const
 228 {
 229         return m_body2->GetWorldPoint(m_localAnchor2);
 230 }
 231
 232 b2Vec2 b2GearJoint::GetReactionForce() const
 233 {
 234         // TODO_ERIN not tested
 235         b2Vec2 F = B2FORCE_SCALE(m_force) * m_J.linear2;
 236         return F;
 237 }
 238
 239 float32 b2GearJoint::GetReactionTorque() const
 240 {
 241         // TODO_ERIN not tested
 242         b2Vec2 r = b2Mul(m_body2->GetXForm().R, m_localAnchor2 - m_body2->GetLocalCenter());
 243         b2Vec2 F = m_force * m_J.linear2;
 244         float32 T = B2FORCE_SCALE(m_force * m_J.angular2 - b2Cross(r, F));
 245         return T;
 246 }
 247
 248 float32 b2GearJoint::GetRatio() const
 249 {
 250         return m_ratio;
 251 }
 252
 253