--- /dev/null
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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 "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(ay1, d)
+// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
+// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+ body1 = b1;
+ body2 = b2;
+ localAnchor1 = body1->GetLocalPoint(anchor);
+ localAnchor2 = body2->GetLocalPoint(anchor);
+ localAxis1 = body1->GetLocalVector(axis);
+ referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchor1 = def->localAnchor1;
+ m_localAnchor2 = def->localAnchor2;
+ m_localXAxis1 = def->localAxis1;
+ m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+ m_refAngle = def->referenceAngle;
+
+ m_linearJacobian.SetZero();
+ m_linearMass = 0.0f;
+ m_force = 0.0f;
+
+ m_angularMass = 0.0f;
+ m_torque = 0.0f;
+
+ m_motorJacobian.SetZero();
+ m_motorMass = 0.0;
+ m_motorForce = 0.0f;
+ m_limitForce = 0.0f;
+ m_limitPositionImpulse = 0.0f;
+
+ m_lowerTranslation = def->lowerTranslation;
+ m_upperTranslation = def->upperTranslation;
+ m_maxMotorForce = B2FORCE_INV_SCALE(def->maxMotorForce);
+ m_motorSpeed = def->motorSpeed;
+ m_enableLimit = def->enableLimit;
+ m_enableMotor = def->enableMotor;
+}
+
+void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ // Compute the effective masses.
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ // Compute point to line constraint effective mass.
+ // J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+ b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+ b2Vec2 e = b2->m_sweep.c + r2 - b1->m_sweep.c; // e = d + r1
+
+ m_linearJacobian.Set(-ay1, -b2Cross(e, ay1), ay1, b2Cross(r2, ay1));
+ m_linearMass = invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 +
+ invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
+ b2Assert(m_linearMass > B2_FLT_EPSILON);
+ m_linearMass = 1.0f / m_linearMass;
+
+ // Compute angular constraint effective mass.
+ m_angularMass = invI1 + invI2;
+ if (m_angularMass > B2_FLT_EPSILON)
+ {
+ m_angularMass = 1.0f / m_angularMass;
+ }
+
+ // Compute motor and limit terms.
+ if (m_enableLimit || m_enableMotor)
+ {
+ // The motor and limit share a Jacobian and effective mass.
+ b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+ m_motorJacobian.Set(-ax1, -b2Cross(e, ax1), ax1, b2Cross(r2, ax1));
+ m_motorMass = invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 +
+ invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
+ b2Assert(m_motorMass > B2_FLT_EPSILON);
+ m_motorMass = 1.0f / m_motorMass;
+
+ if (m_enableLimit)
+ {
+ b2Vec2 d = e - r1; // p2 - p1
+ float32 jointTranslation = b2Dot(ax1, d);
+ if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+ {
+ m_limitState = e_equalLimits;
+ }
+ else if (jointTranslation <= m_lowerTranslation)
+ {
+ if (m_limitState != e_atLowerLimit)
+ {
+ m_limitForce = 0.0f;
+ }
+ m_limitState = e_atLowerLimit;
+ }
+ else if (jointTranslation >= m_upperTranslation)
+ {
+ if (m_limitState != e_atUpperLimit)
+ {
+ m_limitForce = 0.0f;
+ }
+ m_limitState = e_atUpperLimit;
+ }
+ else
+ {
+ m_limitState = e_inactiveLimit;
+ m_limitForce = 0.0f;
+ }
+ }
+ }
+
+ if (m_enableMotor == false)
+ {
+ m_motorForce = 0.0f;
+ }
+
+ if (m_enableLimit == false)
+ {
+ m_limitForce = 0.0f;
+ }
+
+ if (step.warmStarting)
+ {
+ b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear1 + (m_motorForce + m_limitForce) * m_motorJacobian.linear1);
+ b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear2 + (m_motorForce + m_limitForce) * m_motorJacobian.linear2);
+ float32 L1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
+ float32 L2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
+
+ b1->m_linearVelocity += invMass1 * P1;
+ b1->m_angularVelocity += invI1 * L1;
+
+ b2->m_linearVelocity += invMass2 * P2;
+ b2->m_angularVelocity += invI2 * L2;
+ }
+ else
+ {
+ m_force = 0.0f;
+ m_torque = 0.0f;
+ m_limitForce = 0.0f;
+ m_motorForce = 0.0f;
+ }
+
+ m_limitPositionImpulse = 0.0f;
+}
+
+void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ // Solve linear constraint.
+ float32 linearCdot = m_linearJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+ float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_linearMass * linearCdot;
+ m_force += force;
+
+ float32 P = B2FORCE_SCALE(step.dt) * force;
+ b1->m_linearVelocity += (invMass1 * P) * m_linearJacobian.linear1;
+ b1->m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
+
+ b2->m_linearVelocity += (invMass2 * P) * m_linearJacobian.linear2;
+ b2->m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
+
+ // Solve angular constraint.
+ float32 angularCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+ float32 torque = -B2FORCE_INV_SCALE(step.inv_dt) * m_angularMass * angularCdot;
+ m_torque += torque;
+
+ float32 L = B2FORCE_SCALE(step.dt) * torque;
+ b1->m_angularVelocity -= invI1 * L;
+ b2->m_angularVelocity += invI2 * L;
+
+ // Solve linear motor constraint.
+ if (m_enableMotor && m_limitState != e_equalLimits)
+ {
+ float32 motorCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity) - m_motorSpeed;
+ float32 motorForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * motorCdot;
+ float32 oldMotorForce = m_motorForce;
+ m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
+ motorForce = m_motorForce - oldMotorForce;
+
+ float32 P = B2FORCE_SCALE(step.dt) * motorForce;
+ b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+ b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+ b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+ b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+ }
+
+ // Solve linear limit constraint.
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ float32 limitCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+ float32 limitForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * limitCdot;
+
+ if (m_limitState == e_equalLimits)
+ {
+ m_limitForce += limitForce;
+ }
+ else if (m_limitState == e_atLowerLimit)
+ {
+ float32 oldLimitForce = m_limitForce;
+ m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+ limitForce = m_limitForce - oldLimitForce;
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ float32 oldLimitForce = m_limitForce;
+ m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+ limitForce = m_limitForce - oldLimitForce;
+ }
+
+ float32 P = B2FORCE_SCALE(step.dt) * limitForce;
+
+ b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+ b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+ b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+ b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+ }
+}
+
+bool b2PrismaticJoint::SolvePositionConstraints()
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+
+ // Solve linear (point-to-line) constraint.
+ float32 linearC = b2Dot(ay1, d);
+ // Prevent overly large corrections.
+ linearC = b2Clamp(linearC, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+ float32 linearImpulse = -m_linearMass * linearC;
+
+ b1->m_sweep.c += (invMass1 * linearImpulse) * m_linearJacobian.linear1;
+ b1->m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
+ //b1->SynchronizeTransform(); // updated by angular constraint
+ b2->m_sweep.c += (invMass2 * linearImpulse) * m_linearJacobian.linear2;
+ b2->m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
+ //b2->SynchronizeTransform(); // updated by angular constraint
+
+ float32 positionError = b2Abs(linearC);
+
+ // Solve angular constraint.
+ float32 angularC = b2->m_sweep.a - b1->m_sweep.a - m_refAngle;
+ // Prevent overly large corrections.
+ angularC = b2Clamp(angularC, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+ float32 angularImpulse = -m_angularMass * angularC;
+
+ b1->m_sweep.a -= b1->m_invI * angularImpulse;
+ b2->m_sweep.a += b2->m_invI * angularImpulse;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
+ float32 angularError = b2Abs(angularC);
+
+ // Solve linear limit constraint.
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+
+ float32 translation = b2Dot(ax1, d);
+ float32 limitImpulse = 0.0f;
+
+ if (m_limitState == e_equalLimits)
+ {
+ // Prevent large angular corrections
+ float32 limitC = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+ limitImpulse = -m_motorMass * limitC;
+ positionError = b2Max(positionError, b2Abs(angularC));
+ }
+ else if (m_limitState == e_atLowerLimit)
+ {
+ float32 limitC = translation - m_lowerTranslation;
+ positionError = b2Max(positionError, -limitC);
+
+ // Prevent large linear corrections and allow some slop.
+ limitC = b2Clamp(limitC + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ limitImpulse = -m_motorMass * limitC;
+ float32 oldLimitImpulse = m_limitPositionImpulse;
+ m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+ limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ float32 limitC = translation - m_upperTranslation;
+ positionError = b2Max(positionError, limitC);
+
+ // Prevent large linear corrections and allow some slop.
+ limitC = b2Clamp(limitC - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
+ limitImpulse = -m_motorMass * limitC;
+ float32 oldLimitImpulse = m_limitPositionImpulse;
+ m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+ limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+ }
+
+ b1->m_sweep.c += (invMass1 * limitImpulse) * m_motorJacobian.linear1;
+ b1->m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
+ b2->m_sweep.c += (invMass2 * limitImpulse) * m_motorJacobian.linear2;
+ b2->m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+ }
+
+ return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor1() const
+{
+ return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor2() const
+{
+ return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PrismaticJoint::GetReactionForce() const
+{
+ b2Vec2 ax1 = b2Mul(m_body1->GetXForm().R, m_localXAxis1);
+ b2Vec2 ay1 = b2Mul(m_body1->GetXForm().R, m_localYAxis1);
+
+ return B2FORCE_SCALE(float32(1.0))*(m_limitForce * ax1 + m_force * ay1);
+}
+
+float32 b2PrismaticJoint::GetReactionTorque() const
+{
+ return B2FORCE_SCALE(m_torque);
+}
+
+float32 b2PrismaticJoint::GetJointTranslation() const
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+ b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+ float32 translation = b2Dot(d, axis);
+ return translation;
+}
+
+float32 b2PrismaticJoint::GetJointSpeed() const
+{
+ b2Body* b1 = m_body1;
+ b2Body* b2 = m_body2;
+
+ b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+ b2Vec2 v1 = b1->m_linearVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ float32 w1 = b1->m_angularVelocity;
+ float32 w2 = b2->m_angularVelocity;
+
+ float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+ return speed;
+}
+
+bool b2PrismaticJoint::IsLimitEnabled() const
+{
+ return m_enableLimit;
+}
+
+void b2PrismaticJoint::EnableLimit(bool flag)
+{
+ m_enableLimit = flag;
+}
+
+float32 b2PrismaticJoint::GetLowerLimit() const
+{
+ return m_lowerTranslation;
+}
+
+float32 b2PrismaticJoint::GetUpperLimit() const
+{
+ return m_upperTranslation;
+}
+
+void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
+{
+ b2Assert(lower <= upper);
+ m_lowerTranslation = lower;
+ m_upperTranslation = upper;
+}
+
+bool b2PrismaticJoint::IsMotorEnabled() const
+{
+ return m_enableMotor;
+}
+
+void b2PrismaticJoint::EnableMotor(bool flag)
+{
+ m_enableMotor = flag;
+}
+
+void b2PrismaticJoint::SetMotorSpeed(float32 speed)
+{
+ m_motorSpeed = speed;
+}
+
+void b2PrismaticJoint::SetMaxMotorForce(float32 force)
+{
+ m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
+}
+
+float32 b2PrismaticJoint::GetMotorForce() const
+{
+ return m_motorForce;
+}
+
+
+
projects / blok / blobdiff