308 lines
9.6 KiB
JavaScript
308 lines
9.6 KiB
JavaScript
|
/*
|
|||
|
|
* Copyright (c) 2006-2007 Erin Catto http:
|
|||
|
|
*
|
|||
|
|
* 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, and must not be
|
|||
|
|
* misrepresented the original software.
|
|||
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
var b2GearJoint = Class.create();
|
|||
|
|
Object.extend(b2GearJoint.prototype, b2Joint.prototype);
|
|||
|
|
Object.extend(b2GearJoint.prototype,
|
|||
|
|
{
|
|||
|
|
GetAnchor1: function(){
|
|||
|
|
//return this.m_body1.m_position + b2MulMV(this.m_body1.m_R, this.m_localAnchor1);
|
|||
|
|
var tMat = this.m_body1.m_R;
|
|||
|
|
return new b2Vec2( this.m_body1.m_position.x + (tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y),
|
|||
|
|
this.m_body1.m_position.y + (tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y));
|
|||
|
|
},
|
|||
|
|
GetAnchor2: function(){
|
|||
|
|
//return this.m_body2->m_position + b2Mul(this.m_body2->m_R, this.m_localAnchor2);
|
|||
|
|
var tMat = this.m_body2.m_R;
|
|||
|
|
return new b2Vec2( this.m_body2.m_position.x + (tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y),
|
|||
|
|
this.m_body2.m_position.y + (tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y));
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
GetReactionForce: function(invTimeStep){
|
|||
|
|
//b2Vec2 F(0.0f, 0.0f);
|
|||
|
|
return new b2Vec2();
|
|||
|
|
},
|
|||
|
|
GetReactionTorque: function(invTimeStep){
|
|||
|
|
return 0.0;
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
GetRatio: function(){
|
|||
|
|
return this.m_ratio;
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
//--------------- Internals Below -------------------
|
|||
|
|
|
|||
|
|
initialize: function(def){
|
|||
|
|
// The constructor for b2Joint
|
|||
|
|
// initialize instance variables for references
|
|||
|
|
this.m_node1 = new b2JointNode();
|
|||
|
|
this.m_node2 = new b2JointNode();
|
|||
|
|
//
|
|||
|
|
this.m_type = def.type;
|
|||
|
|
this.m_prev = null;
|
|||
|
|
this.m_next = null;
|
|||
|
|
this.m_body1 = def.body1;
|
|||
|
|
this.m_body2 = def.body2;
|
|||
|
|
this.m_collideConnected = def.collideConnected;
|
|||
|
|
this.m_islandFlag = false;
|
|||
|
|
this.m_userData = def.userData;
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
// initialize instance variables for references
|
|||
|
|
this.m_groundAnchor1 = new b2Vec2();
|
|||
|
|
this.m_groundAnchor2 = new b2Vec2();
|
|||
|
|
this.m_localAnchor1 = new b2Vec2();
|
|||
|
|
this.m_localAnchor2 = new b2Vec2();
|
|||
|
|
this.m_J = new b2Jacobian();
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
// parent constructor
|
|||
|
|
//super(def);
|
|||
|
|
|
|||
|
|
//b2Settings.b2Assert(def.joint1.m_type == b2Joint.e_revoluteJoint || def.joint1.m_type == b2Joint.e_prismaticJoint);
|
|||
|
|
//b2Settings.b2Assert(def.joint2.m_type == b2Joint.e_revoluteJoint || def.joint2.m_type == b2Joint.e_prismaticJoint);
|
|||
|
|
//b2Settings.b2Assert(def.joint1.m_body1.IsStatic());
|
|||
|
|
//b2Settings.b2Assert(def.joint2.m_body1.IsStatic());
|
|||
|
|
|
|||
|
|
this.m_revolute1 = null;
|
|||
|
|
this.m_prismatic1 = null;
|
|||
|
|
this.m_revolute2 = null;
|
|||
|
|
this.m_prismatic2 = null;
|
|||
|
|
|
|||
|
|
var coordinate1;
|
|||
|
|
var coordinate2;
|
|||
|
|
|
|||
|
|
this.m_ground1 = def.joint1.m_body1;
|
|||
|
|
this.m_body1 = def.joint1.m_body2;
|
|||
|
|
if (def.joint1.m_type == b2Joint.e_revoluteJoint)
|
|||
|
|
{
|
|||
|
|
this.m_revolute1 = def.joint1;
|
|||
|
|
this.m_groundAnchor1.SetV( this.m_revolute1.m_localAnchor1 );
|
|||
|
|
this.m_localAnchor1.SetV( this.m_revolute1.m_localAnchor2 );
|
|||
|
|
coordinate1 = this.m_revolute1.GetJointAngle();
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
this.m_prismatic1 = def.joint1;
|
|||
|
|
this.m_groundAnchor1.SetV( this.m_prismatic1.m_localAnchor1 );
|
|||
|
|
this.m_localAnchor1.SetV( this.m_prismatic1.m_localAnchor2 );
|
|||
|
|
coordinate1 = this.m_prismatic1.GetJointTranslation();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
this.m_ground2 = def.joint2.m_body1;
|
|||
|
|
this.m_body2 = def.joint2.m_body2;
|
|||
|
|
if (def.joint2.m_type == b2Joint.e_revoluteJoint)
|
|||
|
|
{
|
|||
|
|
this.m_revolute2 = def.joint2;
|
|||
|
|
this.m_groundAnchor2.SetV( this.m_revolute2.m_localAnchor1 );
|
|||
|
|
this.m_localAnchor2.SetV( this.m_revolute2.m_localAnchor2 );
|
|||
|
|
coordinate2 = this.m_revolute2.GetJointAngle();
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
this.m_prismatic2 = def.joint2;
|
|||
|
|
this.m_groundAnchor2.SetV( this.m_prismatic2.m_localAnchor1 );
|
|||
|
|
this.m_localAnchor2.SetV( this.m_prismatic2.m_localAnchor2 );
|
|||
|
|
coordinate2 = this.m_prismatic2.GetJointTranslation();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
this.m_ratio = def.ratio;
|
|||
|
|
|
|||
|
|
this.m_constant = coordinate1 + this.m_ratio * coordinate2;
|
|||
|
|
|
|||
|
|
this.m_impulse = 0.0;
|
|||
|
|
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
PrepareVelocitySolver: function(){
|
|||
|
|
var g1 = this.m_ground1;
|
|||
|
|
var g2 = this.m_ground2;
|
|||
|
|
var b1 = this.m_body1;
|
|||
|
|
var b2 = this.m_body2;
|
|||
|
|
|
|||
|
|
// temp vars
|
|||
|
|
var ugX;
|
|||
|
|
var ugY;
|
|||
|
|
var rX;
|
|||
|
|
var rY;
|
|||
|
|
var tMat;
|
|||
|
|
var tVec;
|
|||
|
|
var crug;
|
|||
|
|
|
|||
|
|
var K = 0.0;
|
|||
|
|
this.m_J.SetZero();
|
|||
|
|
|
|||
|
|
if (this.m_revolute1)
|
|||
|
|
{
|
|||
|
|
this.m_J.angular1 = -1.0;
|
|||
|
|
K += b1.m_invI;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//b2Vec2 ug = b2MulMV(g1->m_R, this.m_prismatic1->m_localXAxis1);
|
|||
|
|
tMat = g1.m_R;
|
|||
|
|
tVec = this.m_prismatic1.m_localXAxis1;
|
|||
|
|
ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
|
|||
|
|
ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
|
|||
|
|
//b2Vec2 r = b2MulMV(b1->m_R, this.m_localAnchor1);
|
|||
|
|
tMat = b1.m_R;
|
|||
|
|
rX = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
|
|||
|
|
rY = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;
|
|||
|
|
|
|||
|
|
//var crug = b2Cross(r, ug);
|
|||
|
|
crug = rX * ugY - rY * ugX;
|
|||
|
|
//this.m_J.linear1 = -ug;
|
|||
|
|
this.m_J.linear1.Set(-ugX, -ugY);
|
|||
|
|
this.m_J.angular1 = -crug;
|
|||
|
|
K += b1.m_invMass + b1.m_invI * crug * crug;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (this.m_revolute2)
|
|||
|
|
{
|
|||
|
|
this.m_J.angular2 = -this.m_ratio;
|
|||
|
|
K += this.m_ratio * this.m_ratio * b2.m_invI;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//b2Vec2 ug = b2Mul(g2->m_R, this.m_prismatic2->m_localXAxis1);
|
|||
|
|
tMat = g2.m_R;
|
|||
|
|
tVec = this.m_prismatic2.m_localXAxis1;
|
|||
|
|
ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
|
|||
|
|
ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
|
|||
|
|
//b2Vec2 r = b2Mul(b2->m_R, this.m_localAnchor2);
|
|||
|
|
tMat = b2.m_R;
|
|||
|
|
rX = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
|
|||
|
|
rY = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;
|
|||
|
|
//float32 crug = b2Cross(r, ug);
|
|||
|
|
crug = rX * ugY - rY * ugX;
|
|||
|
|
//this.m_J.linear2 = -this.m_ratio * ug;
|
|||
|
|
this.m_J.linear2.Set(-this.m_ratio*ugX, -this.m_ratio*ugY);
|
|||
|
|
this.m_J.angular2 = -this.m_ratio * crug;
|
|||
|
|
K += this.m_ratio * this.m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Compute effective mass.
|
|||
|
|
//b2Settings.b2Assert(K > 0.0);
|
|||
|
|
this.m_mass = 1.0 / K;
|
|||
|
|
|
|||
|
|
// Warm starting.
|
|||
|
|
//b1.m_linearVelocity += b1.m_invMass * this.m_impulse * this.m_J.linear1;
|
|||
|
|
b1.m_linearVelocity.x += b1.m_invMass * this.m_impulse * this.m_J.linear1.x;
|
|||
|
|
b1.m_linearVelocity.y += b1.m_invMass * this.m_impulse * this.m_J.linear1.y;
|
|||
|
|
b1.m_angularVelocity += b1.m_invI * this.m_impulse * this.m_J.angular1;
|
|||
|
|
//b2.m_linearVelocity += b2.m_invMass * this.m_impulse * this.m_J.linear2;
|
|||
|
|
b2.m_linearVelocity.x += b2.m_invMass * this.m_impulse * this.m_J.linear2.x;
|
|||
|
|
b2.m_linearVelocity.y += b2.m_invMass * this.m_impulse * this.m_J.linear2.y;
|
|||
|
|
b2.m_angularVelocity += b2.m_invI * this.m_impulse * this.m_J.angular2;
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
|
|||
|
|
SolveVelocityConstraints: function(step){
|
|||
|
|
var b1 = this.m_body1;
|
|||
|
|
var b2 = this.m_body2;
|
|||
|
|
|
|||
|
|
var Cdot = this.m_J.Compute( b1.m_linearVelocity, b1.m_angularVelocity,
|
|||
|
|
b2.m_linearVelocity, b2.m_angularVelocity);
|
|||
|
|
|
|||
|
|
var impulse = -this.m_mass * Cdot;
|
|||
|
|
this.m_impulse += impulse;
|
|||
|
|
|
|||
|
|
b1.m_linearVelocity.x += b1.m_invMass * impulse * this.m_J.linear1.x;
|
|||
|
|
b1.m_linearVelocity.y += b1.m_invMass * impulse * this.m_J.linear1.y;
|
|||
|
|
b1.m_angularVelocity += b1.m_invI * impulse * this.m_J.angular1;
|
|||
|
|
b2.m_linearVelocity.x += b2.m_invMass * impulse * this.m_J.linear2.x;
|
|||
|
|
b2.m_linearVelocity.y += b2.m_invMass * impulse * this.m_J.linear2.y;
|
|||
|
|
b2.m_angularVelocity += b2.m_invI * impulse * this.m_J.angular2;
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
SolvePositionConstraints: function(){
|
|||
|
|
var linearError = 0.0;
|
|||
|
|
|
|||
|
|
var b1 = this.m_body1;
|
|||
|
|
var b2 = this.m_body2;
|
|||
|
|
|
|||
|
|
var coordinate1;
|
|||
|
|
var coordinate2;
|
|||
|
|
if (this.m_revolute1)
|
|||
|
|
{
|
|||
|
|
coordinate1 = this.m_revolute1.GetJointAngle();
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
coordinate1 = this.m_prismatic1.GetJointTranslation();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (this.m_revolute2)
|
|||
|
|
{
|
|||
|
|
coordinate2 = this.m_revolute2.GetJointAngle();
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
coordinate2 = this.m_prismatic2.GetJointTranslation();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
var C = this.m_constant - (coordinate1 + this.m_ratio * coordinate2);
|
|||
|
|
|
|||
|
|
var impulse = -this.m_mass * C;
|
|||
|
|
|
|||
|
|
b1.m_position.x += b1.m_invMass * impulse * this.m_J.linear1.x;
|
|||
|
|
b1.m_position.y += b1.m_invMass * impulse * this.m_J.linear1.y;
|
|||
|
|
b1.m_rotation += b1.m_invI * impulse * this.m_J.angular1;
|
|||
|
|
b2.m_position.x += b2.m_invMass * impulse * this.m_J.linear2.x;
|
|||
|
|
b2.m_position.y += b2.m_invMass * impulse * this.m_J.linear2.y;
|
|||
|
|
b2.m_rotation += b2.m_invI * impulse * this.m_J.angular2;
|
|||
|
|
b1.m_R.Set(b1.m_rotation);
|
|||
|
|
b2.m_R.Set(b2.m_rotation);
|
|||
|
|
|
|||
|
|
return linearError < b2Settings.b2_linearSlop;
|
|||
|
|
},
|
|||
|
|
|
|||
|
|
m_ground1: null,
|
|||
|
|
m_ground2: null,
|
|||
|
|
|
|||
|
|
// One of these is NULL.
|
|||
|
|
m_revolute1: null,
|
|||
|
|
m_prismatic1: null,
|
|||
|
|
|
|||
|
|
// One of these is NULL.
|
|||
|
|
m_revolute2: null,
|
|||
|
|
m_prismatic2: null,
|
|||
|
|
|
|||
|
|
m_groundAnchor1: new b2Vec2(),
|
|||
|
|
m_groundAnchor2: new b2Vec2(),
|
|||
|
|
|
|||
|
|
m_localAnchor1: new b2Vec2(),
|
|||
|
|
m_localAnchor2: new b2Vec2(),
|
|||
|
|
|
|||
|
|
m_J: new b2Jacobian(),
|
|||
|
|
|
|||
|
|
m_constant: null,
|
|||
|
|
m_ratio: null,
|
|||
|
|
|
|||
|
|
// Effective mass
|
|||
|
|
m_mass: null,
|
|||
|
|
|
|||
|
|
// Impulse for accumulation/warm starting.
|
|||
|
|
m_impulse: null});
|
|||
|
|
|