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

   1 /*
   2 * Copyright (c) 2006-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 "b2MouseJoint.h"
  20 #include "../b2Body.h"
  21 #include "../b2World.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         m_target = def->target;
  35         m_localAnchor = b2MulT(m_body2->GetXForm(), m_target);
  36
  37         m_maxForce = B2FORCE_INV_SCALE(def->maxForce);
  38         m_impulse.SetZero();
  39
  40         float32 mass = m_body2->m_mass;
  41
  42         // Frequency
  43         float32 omega = 2.0f * b2_pi * def->frequencyHz;
  44
  45         // Damping coefficient
  46         float32 d = 2.0f * mass * def->dampingRatio * omega;
  47
  48         // Spring stiffness
  49         float32 k = (def->timeStep * mass) * (omega * omega);
  50
  51         // magic formulas
  52         b2Assert(d + k > B2_FLT_EPSILON);
  53         m_gamma = 1.0f / (d + k);
  54         m_beta = k / (d + k);
  55 }
  56
  57 void b2MouseJoint::SetTarget(const b2Vec2& target)
  58 {
  59         if (m_body2->IsSleeping())
  60         {
  61                 m_body2->WakeUp();
  62         }
  63         m_target = target;
  64 }
  65
  66 void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
  67 {
  68         b2Body* b = m_body2;
  69
  70         // Compute the effective mass matrix.
  71         b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
  72
  73         // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
  74         //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
  75         //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
  76         float32 invMass = b->m_invMass;
  77         float32 invI = b->m_invI;
  78
  79         b2Mat22 K1;
  80         K1.col1.x = invMass;    K1.col2.x = 0.0f;
  81         K1.col1.y = 0.0f;               K1.col2.y = invMass;
  82
  83         b2Mat22 K2;
  84         K2.col1.x =  invI * r.y * r.y;  K2.col2.x = -invI * r.x * r.y;
  85         K2.col1.y = -invI * r.x * r.y;  K2.col2.y =  invI * r.x * r.x;
  86
  87         b2Mat22 K = K1 + K2;
  88         K.col1.x += m_gamma;
  89         K.col2.y += m_gamma;
  90
  91         m_mass = K.Invert();
  92
  93         m_C = b->m_sweep.c + r - m_target;
  94
  95         // Cheat with some damping
  96         b->m_angularVelocity *= 0.98f;
  97
  98         // Warm starting.
  99         b2Vec2 P = B2FORCE_SCALE(step.dt) * m_impulse;
 100         b->m_linearVelocity += invMass * P;
 101         b->m_angularVelocity += invI * b2Cross(r, P);
 102 }
 103
 104 void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
 105 {
 106         b2Body* b = m_body2;
 107
 108         b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
 109
 110         // Cdot = v + cross(w, r)
 111         b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
 112         b2Vec2 force = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_mass, Cdot + (m_beta * step.inv_dt) * m_C + B2FORCE_SCALE(step.dt) * (m_gamma * m_impulse));
 113
 114         b2Vec2 oldForce = m_impulse;
 115         m_impulse += force;
 116         float32 forceMagnitude = m_impulse.Length();
 117         if (forceMagnitude > m_maxForce)
 118         {
 119                 m_impulse *= m_maxForce / forceMagnitude;
 120         }
 121         force = m_impulse - oldForce;
 122
 123         b2Vec2 P = B2FORCE_SCALE(step.dt) * force;
 124         b->m_linearVelocity += b->m_invMass * P;
 125         b->m_angularVelocity += b->m_invI * b2Cross(r, P);
 126 }
 127
 128 b2Vec2 b2MouseJoint::GetAnchor1() const
 129 {
 130         return m_target;
 131 }
 132
 133 b2Vec2 b2MouseJoint::GetAnchor2() const
 134 {
 135         return m_body2->GetWorldPoint(m_localAnchor);
 136 }
 137
 138 b2Vec2 b2MouseJoint::GetReactionForce() const
 139 {
 140         return B2FORCE_SCALE(float32(1.0))*m_impulse;
 141 }
 142
 143 float32 b2MouseJoint::GetReactionTorque() const
 144 {
 145         return 0.0f;
 146 }