module.exports = RotationalMotorEquation;
var Vec3 = require('../math/Vec3');
var Mat3 = require('../math/Mat3');
var Equation = require('./Equation');
/**
* Rotational motor constraint. Tries to keep the relative angular velocity of the bodies to a given value.
* @class RotationalMotorEquation
* @constructor
* @author schteppe
* @param {Body} bodyA
* @param {Body} bodyB
* @param {Number} maxForce
* @extends Equation
*/
function RotationalMotorEquation(bodyA, bodyB, maxForce){
maxForce = typeof(maxForce)!=='undefined' ? maxForce : 1e6;
Equation.call(this,bodyA,bodyB,-maxForce,maxForce);
/**
* World oriented rotational axis
* @property {Vec3} axisA
*/
this.axisA = new Vec3();
/**
* World oriented rotational axis
* @property {Vec3} axisB
*/
this.axisB = new Vec3(); // World oriented rotational axis
/**
* Motor velocity
* @property {Number} targetVelocity
*/
this.targetVelocity = 0;
}
RotationalMotorEquation.prototype = new Equation();
RotationalMotorEquation.prototype.constructor = RotationalMotorEquation;
RotationalMotorEquation.prototype.computeB = function(h){
var a = this.a,
b = this.b,
bi = this.bi,
bj = this.bj,
axisA = this.axisA,
axisB = this.axisB,
GA = this.jacobianElementA,
GB = this.jacobianElementB;
// g = 0
// gdot = axisA * wi - axisB * wj
// gdot = G * W = G * [vi wi vj wj]
// =>
// G = [0 axisA 0 -axisB]
GA.rotational.copy(axisA);
axisB.negate(GB.rotational);
var GW = this.computeGW() - this.targetVelocity,
GiMf = this.computeGiMf();
var B = - GW * b - h * GiMf;
return B;
};