/** Copyright (c) 2007-2011 Erin Catto http://www.box2d.org** This software is provided 'as-is', without any express or implied* warranty. In no event will the authors be held liable for any damages* arising from the use of this software.* Permission is granted to anyone to use this software for any purpose,* including commercial applications, and to alter it and redistribute it* freely, subject to the following restrictions:* 1. The origin of this software must not be misrepresented; you must not* claim that you wrote the original software. If you use this software* in a product, an acknowledgment in the product documentation would be* appreciated but is not required.* 2. Altered source versions must be plainly marked as such, and must not be* misrepresented as being the original software.* 3. This notice may not be removed or altered from any source distribution.*/#include <Box2D/Dynamics/Joints/b2RopeJoint.h>#include <Box2D/Dynamics/b2Body.h>#include <Box2D/Dynamics/b2TimeStep.h>// Limit:// C = norm(pB - pA) - L// u = (pB - pA) / norm(pB - pA)// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))// J = [-u -cross(rA, u) u cross(rB, u)]// K = J * invM * JT// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2b2RopeJoint::b2RopeJoint(constb2RopeJointDef*def):b2Joint(def){m_localAnchorA=def->localAnchorA;m_localAnchorB=def->localAnchorB;m_maxLength=def->maxLength;m_mass=0.0f;m_impulse=0.0f;m_state=e_inactiveLimit;m_length=0.0f;}voidb2RopeJoint::InitVelocityConstraints(constb2SolverData&data){m_indexA=m_bodyA->m_islandIndex;m_indexB=m_bodyB->m_islandIndex;m_localCenterA=m_bodyA->m_sweep.localCenter;m_localCenterB=m_bodyB->m_sweep.localCenter;m_invMassA=m_bodyA->m_invMass;m_invMassB=m_bodyB->m_invMass;m_invIA=m_bodyA->m_invI;m_invIB=m_bodyB->m_invI;b2Vec2cA=data.positions[m_indexA].c;float32aA=data.positions[m_indexA].a;b2Vec2vA=data.velocities[m_indexA].v;float32wA=data.velocities[m_indexA].w;b2Vec2cB=data.positions[m_indexB].c;float32aB=data.positions[m_indexB].a;b2Vec2vB=data.velocities[m_indexB].v;float32wB=data.velocities[m_indexB].w;b2RotqA(aA),qB(aB);m_rA=b2Mul(qA,m_localAnchorA-m_localCenterA);m_rB=b2Mul(qB,m_localAnchorB-m_localCenterB);m_u=cB+m_rB-cA-m_rA;m_length=m_u.Length();float32C=m_length-m_maxLength;if(C>0.0f){m_state=e_atUpperLimit;}else{m_state=e_inactiveLimit;}if(m_length>b2_linearSlop){m_u*=1.0f/m_length;}else{m_u.SetZero();m_mass=0.0f;m_impulse=0.0f;return;}// Compute effective mass.float32crA=b2Cross(m_rA,m_u);float32crB=b2Cross(m_rB,m_u);float32invMass=m_invMassA+m_invIA*crA*crA+m_invMassB+m_invIB*crB*crB;m_mass=invMass!=0.0f?1.0f/invMass:0.0f;if(data.step.warmStarting){// Scale the impulse to support a variable time step.m_impulse*=data.step.dtRatio;b2Vec2P=m_impulse*m_u;vA-=m_invMassA*P;wA-=m_invIA*b2Cross(m_rA,P);vB+=m_invMassB*P;wB+=m_invIB*b2Cross(m_rB,P);}else{m_impulse=0.0f;}data.velocities[m_indexA].v=vA;data.velocities[m_indexA].w=wA;data.velocities[m_indexB].v=vB;data.velocities[m_indexB].w=wB;}voidb2RopeJoint::SolveVelocityConstraints(constb2SolverData&data){b2Vec2vA=data.velocities[m_indexA].v;float32wA=data.velocities[m_indexA].w;b2Vec2vB=data.velocities[m_indexB].v;float32wB=data.velocities[m_indexB].w;// Cdot = dot(u, v + cross(w, r))b2Vec2vpA=vA+b2Cross(wA,m_rA);b2Vec2vpB=vB+b2Cross(wB,m_rB);float32C=m_length-m_maxLength;float32Cdot=b2Dot(m_u,vpB-vpA);// Predictive constraint.if(C<0.0f){Cdot+=data.step.inv_dt*C;}float32impulse=-m_mass*Cdot;float32oldImpulse=m_impulse;m_impulse=b2Min(0.0f,m_impulse+impulse);impulse=m_impulse-oldImpulse;b2Vec2P=impulse*m_u;vA-=m_invMassA*P;wA-=m_invIA*b2Cross(m_rA,P);vB+=m_invMassB*P;wB+=m_invIB*b2Cross(m_rB,P);data.velocities[m_indexA].v=vA;data.velocities[m_indexA].w=wA;data.velocities[m_indexB].v=vB;data.velocities[m_indexB].w=wB;}boolb2RopeJoint::SolvePositionConstraints(constb2SolverData&data){b2Vec2cA=data.positions[m_indexA].c;float32aA=data.positions[m_indexA].a;b2Vec2cB=data.positions[m_indexB].c;float32aB=data.positions[m_indexB].a;b2RotqA(aA),qB(aB);b2Vec2rA=b2Mul(qA,m_localAnchorA-m_localCenterA);b2Vec2rB=b2Mul(qB,m_localAnchorB-m_localCenterB);b2Vec2u=cB+rB-cA-rA;float32length=u.Normalize();float32C=length-m_maxLength;C=b2Clamp(C,0.0f,b2_maxLinearCorrection);float32impulse=-m_mass*C;b2Vec2P=impulse*u;cA-=m_invMassA*P;aA-=m_invIA*b2Cross(rA,P);cB+=m_invMassB*P;aB+=m_invIB*b2Cross(rB,P);data.positions[m_indexA].c=cA;data.positions[m_indexA].a=aA;data.positions[m_indexB].c=cB;data.positions[m_indexB].a=aB;returnlength-m_maxLength<b2_linearSlop;}b2Vec2b2RopeJoint::GetAnchorA()const{returnm_bodyA->GetWorldPoint(m_localAnchorA);}b2Vec2b2RopeJoint::GetAnchorB()const{returnm_bodyB->GetWorldPoint(m_localAnchorB);}b2Vec2b2RopeJoint::GetReactionForce(float32inv_dt)const{b2Vec2F=(inv_dt*m_impulse)*m_u;returnF;}float32b2RopeJoint::GetReactionTorque(float32inv_dt)const{B2_NOT_USED(inv_dt);return0.0f;}float32b2RopeJoint::GetMaxLength()const{returnm_maxLength;}b2LimitStateb2RopeJoint::GetLimitState()const{returnm_state;}voidb2RopeJoint::Dump(){int32indexA=m_bodyA->m_islandIndex;int32indexB=m_bodyB->m_islandIndex;b2Log(" b2RopeJointDef jd;\n");b2Log(" jd.bodyA = bodies[%d];\n",indexA);b2Log(" jd.bodyB = bodies[%d];\n",indexB);b2Log(" jd.collideConnected = bool(%d);\n",m_collideConnected);b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n",m_localAnchorA.x,m_localAnchorA.y);b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n",m_localAnchorB.x,m_localAnchorB.y);b2Log(" jd.maxLength = %.15lef;\n",m_maxLength);b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n",m_index);}