var Constraint = require('./Constraint')
, Equation = require('../equations/Equation')
, RotationalVelocityEquation = require('../equations/RotationalVelocityEquation')
, RotationalLockEquation = require('../equations/RotationalLockEquation')
, vec2 = require('../math/vec2');
module.exports = RevoluteConstraint;
var worldPivotA = vec2.create(),
worldPivotB = vec2.create(),
xAxis = vec2.fromValues(1,0),
yAxis = vec2.fromValues(0,1),
g = vec2.create();
/**
* Connects two bodies at given offset points, letting them rotate relative to each other around this point.
* @class RevoluteConstraint
* @constructor
* @author schteppe
* @param {Body} bodyA
* @param {Body} bodyB
* @param {Object} [options]
* @param {Array} [options.worldPivot] A pivot point given in world coordinates. If specified, localPivotA and localPivotB are automatically computed from this value.
* @param {Array} [options.localPivotA] The point relative to the center of mass of bodyA which bodyA is constrained to.
* @param {Array} [options.localPivotB] See localPivotA.
* @param {Number} [options.maxForce] The maximum force that should be applied to constrain the bodies.
* @extends Constraint
*
* @example
* // This will create a revolute constraint between two bodies with pivot point in between them.
* var bodyA = new Body({ mass: 1, position: [-1, 0] });
* var bodyB = new Body({ mass: 1, position: [1, 0] });
* var constraint = new RevoluteConstraint(bodyA, bodyB, {
* worldPivot: [0, 0]
* });
* world.addConstraint(constraint);
*
* // Using body-local pivot points, the constraint could have been constructed like this:
* var constraint = new RevoluteConstraint(bodyA, bodyB, {
* localPivotA: [1, 0],
* localPivotB: [-1, 0]
* });
*/
function RevoluteConstraint(bodyA, bodyB, options){
options = options || {};
Constraint.call(this,bodyA,bodyB,Constraint.REVOLUTE,options);
var maxForce = this.maxForce = typeof(options.maxForce) !== "undefined" ? options.maxForce : Number.MAX_VALUE;
/**
* @property {Array} pivotA
*/
this.pivotA = vec2.create();
/**
* @property {Array} pivotB
*/
this.pivotB = vec2.create();
if(options.worldPivot){
// Compute pivotA and pivotB
vec2.sub(this.pivotA, options.worldPivot, bodyA.position);
vec2.sub(this.pivotB, options.worldPivot, bodyB.position);
// Rotate to local coordinate system
vec2.rotate(this.pivotA, this.pivotA, -bodyA.angle);
vec2.rotate(this.pivotB, this.pivotB, -bodyB.angle);
} else {
// Get pivotA and pivotB
vec2.copy(this.pivotA, options.localPivotA);
vec2.copy(this.pivotB, options.localPivotB);
}
// Equations to be fed to the solver
var eqs = this.equations = [
new Equation(bodyA,bodyB,-maxForce,maxForce),
new Equation(bodyA,bodyB,-maxForce,maxForce),
];
var x = eqs[0];
var y = eqs[1];
var that = this;
x.computeGq = function(){
vec2.rotate(worldPivotA, that.pivotA, bodyA.angle);
vec2.rotate(worldPivotB, that.pivotB, bodyB.angle);
vec2.add(g, bodyB.position, worldPivotB);
vec2.sub(g, g, bodyA.position);
vec2.sub(g, g, worldPivotA);
return vec2.dot(g,xAxis);
};
y.computeGq = function(){
vec2.rotate(worldPivotA, that.pivotA, bodyA.angle);
vec2.rotate(worldPivotB, that.pivotB, bodyB.angle);
vec2.add(g, bodyB.position, worldPivotB);
vec2.sub(g, g, bodyA.position);
vec2.sub(g, g, worldPivotA);
return vec2.dot(g,yAxis);
};
y.minForce = x.minForce = -maxForce;
y.maxForce = x.maxForce = maxForce;
this.motorEquation = new RotationalVelocityEquation(bodyA,bodyB);
/**
* Indicates whether the motor is enabled. Use .enableMotor() to enable the constraint motor.
* @property {Boolean} motorEnabled
* @readOnly
*/
this.motorEnabled = false;
/**
* The constraint position.
* @property angle
* @type {Number}
* @readOnly
*/
this.angle = 0;
/**
* Set to true to enable lower limit
* @property lowerLimitEnabled
* @type {Boolean}
*/
this.lowerLimitEnabled = false;
/**
* Set to true to enable upper limit
* @property upperLimitEnabled
* @type {Boolean}
*/
this.upperLimitEnabled = false;
/**
* The lower limit on the constraint angle.
* @property lowerLimit
* @type {Boolean}
*/
this.lowerLimit = 0;
/**
* The upper limit on the constraint angle.
* @property upperLimit
* @type {Boolean}
*/
this.upperLimit = 0;
this.upperLimitEquation = new RotationalLockEquation(bodyA,bodyB);
this.lowerLimitEquation = new RotationalLockEquation(bodyA,bodyB);
this.upperLimitEquation.minForce = 0;
this.lowerLimitEquation.maxForce = 0;
}
RevoluteConstraint.prototype = new Constraint();
RevoluteConstraint.prototype.constructor = RevoluteConstraint;
/**
* Set the constraint angle limits.
* @method setLimits
* @param {number} lower Lower angle limit.
* @param {number} upper Upper angle limit.
*/
RevoluteConstraint.prototype.setLimits = function (lower, upper) {
if(typeof(lower) === 'number'){
this.lowerLimit = lower;
this.lowerLimitEnabled = true;
} else {
this.lowerLimit = lower;
this.lowerLimitEnabled = false;
}
if(typeof(upper) === 'number'){
this.upperLimit = upper;
this.upperLimitEnabled = true;
} else {
this.upperLimit = upper;
this.upperLimitEnabled = false;
}
};
RevoluteConstraint.prototype.update = function(){
var bodyA = this.bodyA,
bodyB = this.bodyB,
pivotA = this.pivotA,
pivotB = this.pivotB,
eqs = this.equations,
normal = eqs[0],
tangent= eqs[1],
x = eqs[0],
y = eqs[1],
upperLimit = this.upperLimit,
lowerLimit = this.lowerLimit,
upperLimitEquation = this.upperLimitEquation,
lowerLimitEquation = this.lowerLimitEquation;
var relAngle = this.angle = bodyB.angle - bodyA.angle;
if(this.upperLimitEnabled && relAngle > upperLimit){
upperLimitEquation.angle = upperLimit;
if(eqs.indexOf(upperLimitEquation) === -1){
eqs.push(upperLimitEquation);
}
} else {
var idx = eqs.indexOf(upperLimitEquation);
if(idx !== -1){
eqs.splice(idx,1);
}
}
if(this.lowerLimitEnabled && relAngle < lowerLimit){
lowerLimitEquation.angle = lowerLimit;
if(eqs.indexOf(lowerLimitEquation) === -1){
eqs.push(lowerLimitEquation);
}
} else {
var idx = eqs.indexOf(lowerLimitEquation);
if(idx !== -1){
eqs.splice(idx,1);
}
}
/*
The constraint violation is
g = xj + rj - xi - ri
...where xi and xj are the body positions and ri and rj world-oriented offset vectors. Differentiate:
gdot = vj + wj x rj - vi - wi x ri
We split this into x and y directions. (let x and y be unit vectors along the respective axes)
gdot * x = ( vj + wj x rj - vi - wi x ri ) * x
= ( vj*x + (wj x rj)*x -vi*x -(wi x ri)*x
= ( vj*x + (rj x x)*wj -vi*x -(ri x x)*wi
= [ -x -(ri x x) x (rj x x)] * [vi wi vj wj]
= G*W
...and similar for y. We have then identified the jacobian entries for x and y directions:
Gx = [ x (rj x x) -x -(ri x x)]
Gy = [ y (rj x y) -y -(ri x y)]
*/
vec2.rotate(worldPivotA, pivotA, bodyA.angle);
vec2.rotate(worldPivotB, pivotB, bodyB.angle);
// todo: these are a bit sparse. We could save some computations on making custom eq.computeGW functions, etc
x.G[0] = -1;
x.G[1] = 0;
x.G[2] = -vec2.crossLength(worldPivotA,xAxis);
x.G[3] = 1;
x.G[4] = 0;
x.G[5] = vec2.crossLength(worldPivotB,xAxis);
y.G[0] = 0;
y.G[1] = -1;
y.G[2] = -vec2.crossLength(worldPivotA,yAxis);
y.G[3] = 0;
y.G[4] = 1;
y.G[5] = vec2.crossLength(worldPivotB,yAxis);
};
/**
* Enable the rotational motor
* @method enableMotor
*/
RevoluteConstraint.prototype.enableMotor = function(){
if(this.motorEnabled){
return;
}
this.equations.push(this.motorEquation);
this.motorEnabled = true;
};
/**
* Disable the rotational motor
* @method disableMotor
*/
RevoluteConstraint.prototype.disableMotor = function(){
if(!this.motorEnabled){
return;
}
var i = this.equations.indexOf(this.motorEquation);
this.equations.splice(i,1);
this.motorEnabled = false;
};
/**
* Check if the motor is enabled.
* @method motorIsEnabled
* @deprecated use property motorEnabled instead.
* @return {Boolean}
*/
RevoluteConstraint.prototype.motorIsEnabled = function(){
return !!this.motorEnabled;
};
/**
* Set the speed of the rotational constraint motor
* @method setMotorSpeed
* @param {Number} speed
*/
RevoluteConstraint.prototype.setMotorSpeed = function(speed){
if(!this.motorEnabled){
return;
}
var i = this.equations.indexOf(this.motorEquation);
this.equations[i].relativeVelocity = speed;
};
/**
* Get the speed of the rotational constraint motor
* @method getMotorSpeed
* @return {Number} The current speed, or false if the motor is not enabled.
*/
RevoluteConstraint.prototype.getMotorSpeed = function(){
if(!this.motorEnabled){
return false;
}
return this.motorEquation.relativeVelocity;
};
