module.exports = ConeTwistConstraint;
var Constraint = require('./Constraint');
var PointToPointConstraint = require('./PointToPointConstraint');
var ConeEquation = require('../equations/ConeEquation');
var RotationalEquation = require('../equations/RotationalEquation');
var ContactEquation = require('../equations/ContactEquation');
var Vec3 = require('../math/Vec3');
/**
* @class ConeTwistConstraint
* @constructor
* @author schteppe
* @param {Body} bodyA
* @param {Body} bodyB
* @param {object} [options]
* @param {Vec3} [options.pivotA]
* @param {Vec3} [options.pivotB]
* @param {Vec3} [options.axisA]
* @param {Vec3} [options.axisB]
* @param {Number} [options.maxForce=1e6]
* @extends PointToPointConstraint
*/
function ConeTwistConstraint(bodyA, bodyB, options){
options = options || {};
var maxForce = typeof(options.maxForce) !== 'undefined' ? options.maxForce : 1e6;
// Set pivot point in between
var pivotA = options.pivotA ? options.pivotA.clone() : new Vec3();
var pivotB = options.pivotB ? options.pivotB.clone() : new Vec3();
this.axisA = options.axisA ? options.axisA.clone() : new Vec3();
this.axisB = options.axisB ? options.axisB.clone() : new Vec3();
PointToPointConstraint.call(this, bodyA, pivotA, bodyB, pivotB, maxForce);
this.collideConnected = !!options.collideConnected;
this.angle = typeof(options.angle) !== 'undefined' ? options.angle : 0;
/**
* @property {ConeEquation} coneEquation
*/
var c = this.coneEquation = new ConeEquation(bodyA,bodyB,options);
/**
* @property {RotationalEquation} twistEquation
*/
var t = this.twistEquation = new RotationalEquation(bodyA,bodyB,options);
this.twistAngle = typeof(options.twistAngle) !== 'undefined' ? options.twistAngle : 0;
// Make the cone equation push the bodies toward the cone axis, not outward
c.maxForce = 0;
c.minForce = -maxForce;
// Make the twist equation add torque toward the initial position
t.maxForce = 0;
t.minForce = -maxForce;
this.equations.push(c, t);
}
ConeTwistConstraint.prototype = new PointToPointConstraint();
ConeTwistConstraint.constructor = ConeTwistConstraint;
var ConeTwistConstraint_update_tmpVec1 = new Vec3();
var ConeTwistConstraint_update_tmpVec2 = new Vec3();
ConeTwistConstraint.prototype.update = function(){
var bodyA = this.bodyA,
bodyB = this.bodyB,
cone = this.coneEquation,
twist = this.twistEquation;
PointToPointConstraint.prototype.update.call(this);
// Update the axes to the cone constraint
bodyA.vectorToWorldFrame(this.axisA, cone.axisA);
bodyB.vectorToWorldFrame(this.axisB, cone.axisB);
// Update the world axes in the twist constraint
this.axisA.tangents(twist.axisA, twist.axisA);
bodyA.vectorToWorldFrame(twist.axisA, twist.axisA);
this.axisB.tangents(twist.axisB, twist.axisB);
bodyB.vectorToWorldFrame(twist.axisB, twist.axisB);
cone.angle = this.angle;
twist.maxAngle = this.twistAngle;
};