/*
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
*
* Bullet Continuous Collision Detection and Physics Library
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.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.
*/
/*
Added by Roman Ponomarev (rponom@gmail.com)
April 04, 2008
TODO:
- add clamping od accumulated impulse to improve stability
- add conversion for ODE constraint solver
*/
package com.bulletphysics.dynamics.constraintsolver;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.linearmath.VectorUtil;
import cz.advel.stack.Stack;
import javax.vecmath.Matrix3f;
import javax.vecmath.Vector3f;
// JAVA NOTE: SliderConstraint from 2.71
/**
*
* @author jezek2
*/
public class SliderConstraint extends TypedConstraint {
public static final float SLIDER_CONSTRAINT_DEF_SOFTNESS = 1.0f;
public static final float SLIDER_CONSTRAINT_DEF_DAMPING = 1.0f;
public static final float SLIDER_CONSTRAINT_DEF_RESTITUTION = 0.7f;
protected final Transform frameInA = new Transform();
protected final Transform frameInB = new Transform();
// use frameA fo define limits, if true
protected boolean useLinearReferenceFrameA;
// linear limits
protected float lowerLinLimit;
protected float upperLinLimit;
// angular limits
protected float lowerAngLimit;
protected float upperAngLimit;
// softness, restitution and damping for different cases
// DirLin - moving inside linear limits
// LimLin - hitting linear limit
// DirAng - moving inside angular limits
// LimAng - hitting angular limit
// OrthoLin, OrthoAng - against constraint axis
protected float softnessDirLin;
protected float restitutionDirLin;
protected float dampingDirLin;
protected float softnessDirAng;
protected float restitutionDirAng;
protected float dampingDirAng;
protected float softnessLimLin;
protected float restitutionLimLin;
protected float dampingLimLin;
protected float softnessLimAng;
protected float restitutionLimAng;
protected float dampingLimAng;
protected float softnessOrthoLin;
protected float restitutionOrthoLin;
protected float dampingOrthoLin;
protected float softnessOrthoAng;
protected float restitutionOrthoAng;
protected float dampingOrthoAng;
// for interlal use
protected boolean solveLinLim;
protected boolean solveAngLim;
protected JacobianEntry[] jacLin = new JacobianEntry[/*3*/] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() };
protected float[] jacLinDiagABInv = new float[3];
protected JacobianEntry[] jacAng = new JacobianEntry[/*3*/] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() };
protected float timeStep;
protected final Transform calculatedTransformA = new Transform();
protected final Transform calculatedTransformB = new Transform();
protected final Vector3f sliderAxis = new Vector3f();
protected final Vector3f realPivotAInW = new Vector3f();
protected final Vector3f realPivotBInW = new Vector3f();
protected final Vector3f projPivotInW = new Vector3f();
protected final Vector3f delta = new Vector3f();
protected final Vector3f depth = new Vector3f();
protected final Vector3f relPosA = new Vector3f();
protected final Vector3f relPosB = new Vector3f();
protected float linPos;
protected float angDepth;
protected float kAngle;
protected boolean poweredLinMotor;
protected float targetLinMotorVelocity;
protected float maxLinMotorForce;
protected float accumulatedLinMotorImpulse;
protected boolean poweredAngMotor;
protected float targetAngMotorVelocity;
protected float maxAngMotorForce;
protected float accumulatedAngMotorImpulse;
public SliderConstraint() {
super(TypedConstraintType.SLIDER_CONSTRAINT_TYPE);
useLinearReferenceFrameA = true;
initParams();
}
public SliderConstraint(RigidBody rbA, RigidBody rbB, Transform frameInA, Transform frameInB ,boolean useLinearReferenceFrameA) {
super(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, rbA, rbB);
this.frameInA.set(frameInA);
this.frameInB.set(frameInB);
this.useLinearReferenceFrameA = useLinearReferenceFrameA;
initParams();
}
protected void initParams() {
lowerLinLimit = 1f;
upperLinLimit = -1f;
lowerAngLimit = 0f;
upperAngLimit = 0f;
softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
dampingDirLin = 0f;
softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
dampingDirAng = 0f;
softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
poweredLinMotor = false;
targetLinMotorVelocity = 0f;
maxLinMotorForce = 0f;
accumulatedLinMotorImpulse = 0f;
poweredAngMotor = false;
targetAngMotorVelocity = 0f;
maxAngMotorForce = 0f;
accumulatedAngMotorImpulse = 0f;
}
@Override
public void buildJacobian() {
if (useLinearReferenceFrameA) {
buildJacobianInt(rbA, rbB, frameInA, frameInB);
}
else {
buildJacobianInt(rbB, rbA, frameInB, frameInA);
}
}
@Override
public void solveConstraint(float timeStep) {
this.timeStep = timeStep;
if (useLinearReferenceFrameA) {
solveConstraintInt(rbA, rbB);
}
else {
solveConstraintInt(rbB, rbA);
}
}
public Transform getCalculatedTransformA(Transform out) {
out.set(calculatedTransformA);
return out;
}
public Transform getCalculatedTransformB(Transform out) {
out.set(calculatedTransformB);
return out;
}
public Transform getFrameOffsetA(Transform out) {
out.set(frameInA);
return out;
}
public Transform getFrameOffsetB(Transform out) {
out.set(frameInB);
return out;
}
public float getLowerLinLimit() {
return lowerLinLimit;
}
public void setLowerLinLimit(float lowerLimit) {
this.lowerLinLimit = lowerLimit;
}
public float getUpperLinLimit() {
return upperLinLimit;
}
public void setUpperLinLimit(float upperLimit) {
this.upperLinLimit = upperLimit;
}
public float getLowerAngLimit() {
return lowerAngLimit;
}
public void setLowerAngLimit(float lowerLimit) {
this.lowerAngLimit = lowerLimit;
}
public float getUpperAngLimit() {
return upperAngLimit;
}
public void setUpperAngLimit(float upperLimit) {
this.upperAngLimit = upperLimit;
}
public boolean getUseLinearReferenceFrameA() {
return useLinearReferenceFrameA;
}
public float getSoftnessDirLin() {
return softnessDirLin;
}
public float getRestitutionDirLin() {
return restitutionDirLin;
}
public float getDampingDirLin() {
return dampingDirLin;
}
public float getSoftnessDirAng() {
return softnessDirAng;
}
public float getRestitutionDirAng() {
return restitutionDirAng;
}
public float getDampingDirAng() {
return dampingDirAng;
}
public float getSoftnessLimLin() {
return softnessLimLin;
}
public float getRestitutionLimLin() {
return restitutionLimLin;
}
public float getDampingLimLin() {
return dampingLimLin;
}
public float getSoftnessLimAng() {
return softnessLimAng;
}
public float getRestitutionLimAng() {
return restitutionLimAng;
}
public float getDampingLimAng() {
return dampingLimAng;
}
public float getSoftnessOrthoLin() {
return softnessOrthoLin;
}
public float getRestitutionOrthoLin() {
return restitutionOrthoLin;
}
public float getDampingOrthoLin() {
return dampingOrthoLin;
}
public float getSoftnessOrthoAng() {
return softnessOrthoAng;
}
public float getRestitutionOrthoAng() {
return restitutionOrthoAng;
}
public float getDampingOrthoAng() {
return dampingOrthoAng;
}
public void setSoftnessDirLin(float softnessDirLin) {
this.softnessDirLin = softnessDirLin;
}
public void setRestitutionDirLin(float restitutionDirLin) {
this.restitutionDirLin = restitutionDirLin;
}
public void setDampingDirLin(float dampingDirLin) {
this.dampingDirLin = dampingDirLin;
}
public void setSoftnessDirAng(float softnessDirAng) {
this.softnessDirAng = softnessDirAng;
}
public void setRestitutionDirAng(float restitutionDirAng) {
this.restitutionDirAng = restitutionDirAng;
}
public void setDampingDirAng(float dampingDirAng) {
this.dampingDirAng = dampingDirAng;
}
public void setSoftnessLimLin(float softnessLimLin) {
this.softnessLimLin = softnessLimLin;
}
public void setRestitutionLimLin(float restitutionLimLin) {
this.restitutionLimLin = restitutionLimLin;
}
public void setDampingLimLin(float dampingLimLin) {
this.dampingLimLin = dampingLimLin;
}
public void setSoftnessLimAng(float softnessLimAng) {
this.softnessLimAng = softnessLimAng;
}
public void setRestitutionLimAng(float restitutionLimAng) {
this.restitutionLimAng = restitutionLimAng;
}
public void setDampingLimAng(float dampingLimAng) {
this.dampingLimAng = dampingLimAng;
}
public void setSoftnessOrthoLin(float softnessOrthoLin) {
this.softnessOrthoLin = softnessOrthoLin;
}
public void setRestitutionOrthoLin(float restitutionOrthoLin) {
this.restitutionOrthoLin = restitutionOrthoLin;
}
public void setDampingOrthoLin(float dampingOrthoLin) {
this.dampingOrthoLin = dampingOrthoLin;
}
public void setSoftnessOrthoAng(float softnessOrthoAng) {
this.softnessOrthoAng = softnessOrthoAng;
}
public void setRestitutionOrthoAng(float restitutionOrthoAng) {
this.restitutionOrthoAng = restitutionOrthoAng;
}
public void setDampingOrthoAng(float dampingOrthoAng) {
this.dampingOrthoAng = dampingOrthoAng;
}
public void setPoweredLinMotor(boolean onOff) {
this.poweredLinMotor = onOff;
}
public boolean getPoweredLinMotor() {
return poweredLinMotor;
}
public void setTargetLinMotorVelocity(float targetLinMotorVelocity) {
this.targetLinMotorVelocity = targetLinMotorVelocity;
}
public float getTargetLinMotorVelocity() {
return targetLinMotorVelocity;
}
public void setMaxLinMotorForce(float maxLinMotorForce) {
this.maxLinMotorForce = maxLinMotorForce;
}
public float getMaxLinMotorForce() {
return maxLinMotorForce;
}
public void setPoweredAngMotor(boolean onOff) {
this.poweredAngMotor = onOff;
}
public boolean getPoweredAngMotor() {
return poweredAngMotor;
}
public void setTargetAngMotorVelocity(float targetAngMotorVelocity) {
this.targetAngMotorVelocity = targetAngMotorVelocity;
}
public float getTargetAngMotorVelocity() {
return targetAngMotorVelocity;
}
public void setMaxAngMotorForce(float maxAngMotorForce) {
this.maxAngMotorForce = maxAngMotorForce;
}
public float getMaxAngMotorForce() {
return this.maxAngMotorForce;
}
public float getLinearPos() {
return this.linPos;
}
// access for ODE solver
public boolean getSolveLinLimit() {
return solveLinLim;
}
public float getLinDepth() {
return depth.x;
}
public boolean getSolveAngLimit() {
return solveAngLim;
}
public float getAngDepth() {
return angDepth;
}
// internal
public void buildJacobianInt(RigidBody rbA, RigidBody rbB, Transform frameInA, Transform frameInB) {
Transform tmpTrans = Stack.alloc(Transform.class);
Transform tmpTrans1 = Stack.alloc(Transform.class);
Transform tmpTrans2 = Stack.alloc(Transform.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
// calculate transforms
calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
realPivotAInW.set(calculatedTransformA.origin);
realPivotBInW.set(calculatedTransformB.origin);
calculatedTransformA.basis.getColumn(0, tmp);
sliderAxis.set(tmp); // along X
delta.sub(realPivotBInW, realPivotAInW);
projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
relPosA.sub(projPivotInW, rbA.getCenterOfMassPosition(tmp));
relPosB.sub(realPivotBInW, rbB.getCenterOfMassPosition(tmp));
Vector3f normalWorld = Stack.alloc(Vector3f.class);
// linear part
for (int i=0; i<3; i++) {
calculatedTransformA.basis.getColumn(i, normalWorld);
Matrix3f mat1 = rbA.getCenterOfMassTransform(tmpTrans1).basis;
mat1.transpose();
Matrix3f mat2 = rbB.getCenterOfMassTransform(tmpTrans2).basis;
mat2.transpose();
jacLin[i].init(
mat1,
mat2,
relPosA,
relPosB,
normalWorld,
rbA.getInvInertiaDiagLocal(tmp),
rbA.getInvMass(),
rbB.getInvInertiaDiagLocal(tmp2),
rbB.getInvMass());
jacLinDiagABInv[i] = 1f / jacLin[i].getDiagonal();
VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
}
testLinLimits();
// angular part
for (int i=0; i<3; i++) {
calculatedTransformA.basis.getColumn(i, normalWorld);
Matrix3f mat1 = rbA.getCenterOfMassTransform(tmpTrans1).basis;
mat1.transpose();
Matrix3f mat2 = rbB.getCenterOfMassTransform(tmpTrans2).basis;
mat2.transpose();
jacAng[i].init(
normalWorld,
mat1,
mat2,
rbA.getInvInertiaDiagLocal(tmp),
rbB.getInvInertiaDiagLocal(tmp2));
}
testAngLimits();
Vector3f axisA = Stack.alloc(Vector3f.class);
calculatedTransformA.basis.getColumn(0, axisA);
kAngle = 1f / (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
// clear accumulator for motors
accumulatedLinMotorImpulse = 0f;
accumulatedAngMotorImpulse = 0f;
}
public void solveConstraintInt(RigidBody rbA, RigidBody rbB) {
Vector3f tmp = Stack.alloc(Vector3f.class);
// linear
Vector3f velA = rbA.getVelocityInLocalPoint(relPosA, Stack.alloc(Vector3f.class));
Vector3f velB = rbB.getVelocityInLocalPoint(relPosB, Stack.alloc(Vector3f.class));
Vector3f vel = Stack.alloc(Vector3f.class);
vel.sub(velA, velB);
Vector3f impulse_vector = Stack.alloc(Vector3f.class);
for (int i=0; i<3; i++) {
Vector3f normal = jacLin[i].linearJointAxis;
float rel_vel = normal.dot(vel);
// calculate positional error
float depth = VectorUtil.getCoord(this.depth, i);
// get parameters
float softness = (i != 0)? softnessOrthoLin : (solveLinLim? softnessLimLin : softnessDirLin);
float restitution = (i != 0)? restitutionOrthoLin : (solveLinLim? restitutionLimLin : restitutionDirLin);
float damping = (i != 0)? dampingOrthoLin : (solveLinLim? dampingLimLin : dampingDirLin);
// calcutate and apply impulse
float normalImpulse = softness * (restitution * depth / timeStep - damping * rel_vel) * jacLinDiagABInv[i];
impulse_vector.scale(normalImpulse, normal);
rbA.applyImpulse(impulse_vector, relPosA);
tmp.negate(impulse_vector);
rbB.applyImpulse(tmp, relPosB);
if (poweredLinMotor && (i == 0)) {
// apply linear motor
if (accumulatedLinMotorImpulse < maxLinMotorForce) {
float desiredMotorVel = targetLinMotorVelocity;
float motor_relvel = desiredMotorVel + rel_vel;
normalImpulse = -motor_relvel * jacLinDiagABInv[i];
// clamp accumulated impulse
float new_acc = accumulatedLinMotorImpulse + Math.abs(normalImpulse);
if (new_acc > maxLinMotorForce) {
new_acc = maxLinMotorForce;
}
float del = new_acc - accumulatedLinMotorImpulse;
if (normalImpulse < 0f) {
normalImpulse = -del;
}
else {
normalImpulse = del;
}
accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector.scale(normalImpulse, normal);
rbA.applyImpulse(impulse_vector, relPosA);
tmp.negate(impulse_vector);
rbB.applyImpulse(tmp, relPosB);
}
}
}
// angular
// get axes in world space
Vector3f axisA = Stack.alloc(Vector3f.class);
calculatedTransformA.basis.getColumn(0, axisA);
Vector3f axisB = Stack.alloc(Vector3f.class);
calculatedTransformB.basis.getColumn(0, axisB);
Vector3f angVelA = rbA.getAngularVelocity(Stack.alloc(Vector3f.class));
Vector3f angVelB = rbB.getAngularVelocity(Stack.alloc(Vector3f.class));
Vector3f angVelAroundAxisA = Stack.alloc(Vector3f.class);
angVelAroundAxisA.scale(axisA.dot(angVelA), axisA);
Vector3f angVelAroundAxisB = Stack.alloc(Vector3f.class);
angVelAroundAxisB.scale(axisB.dot(angVelB), axisB);
Vector3f angAorthog = Stack.alloc(Vector3f.class);
angAorthog.sub(angVelA, angVelAroundAxisA);
Vector3f angBorthog = Stack.alloc(Vector3f.class);
angBorthog.sub(angVelB, angVelAroundAxisB);
Vector3f velrelOrthog = Stack.alloc(Vector3f.class);
velrelOrthog.sub(angAorthog, angBorthog);
// solve orthogonal angular velocity correction
float len = velrelOrthog.length();
if (len > 0.00001f) {
Vector3f normal = Stack.alloc(Vector3f.class);
normal.normalize(velrelOrthog);
float denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
velrelOrthog.scale((1f / denom) * dampingOrthoAng * softnessOrthoAng);
}
// solve angular positional correction
Vector3f angularError = Stack.alloc(Vector3f.class);
angularError.cross(axisA, axisB);
angularError.scale(1f / timeStep);
float len2 = angularError.length();
if (len2 > 0.00001f) {
Vector3f normal2 = Stack.alloc(Vector3f.class);
normal2.normalize(angularError);
float denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
angularError.scale((1f / denom2) * restitutionOrthoAng * softnessOrthoAng);
}
// apply impulse
tmp.negate(velrelOrthog);
tmp.add(angularError);
rbA.applyTorqueImpulse(tmp);
tmp.sub(velrelOrthog, angularError);
rbB.applyTorqueImpulse(tmp);
float impulseMag;
// solve angular limits
if (solveAngLim) {
tmp.sub(angVelB, angVelA);
impulseMag = tmp.dot(axisA) * dampingLimAng + angDepth * restitutionLimAng / timeStep;
impulseMag *= kAngle * softnessLimAng;
}
else {
tmp.sub(angVelB, angVelA);
impulseMag = tmp.dot(axisA) * dampingDirAng + angDepth * restitutionDirAng / timeStep;
impulseMag *= kAngle * softnessDirAng;
}
Vector3f impulse = Stack.alloc(Vector3f.class);
impulse.scale(impulseMag, axisA);
rbA.applyTorqueImpulse(impulse);
tmp.negate(impulse);
rbB.applyTorqueImpulse(tmp);
// apply angular motor
if (poweredAngMotor) {
if (accumulatedAngMotorImpulse < maxAngMotorForce) {
Vector3f velrel = Stack.alloc(Vector3f.class);
velrel.sub(angVelAroundAxisA, angVelAroundAxisB);
float projRelVel = velrel.dot(axisA);
float desiredMotorVel = targetAngMotorVelocity;
float motor_relvel = desiredMotorVel - projRelVel;
float angImpulse = kAngle * motor_relvel;
// clamp accumulated impulse
float new_acc = accumulatedAngMotorImpulse + Math.abs(angImpulse);
if (new_acc > maxAngMotorForce) {
new_acc = maxAngMotorForce;
}
float del = new_acc - accumulatedAngMotorImpulse;
if (angImpulse < 0f) {
angImpulse = -del;
} else {
angImpulse = del;
}
accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse
Vector3f motorImp = Stack.alloc(Vector3f.class);
motorImp.scale(angImpulse, axisA);
rbA.applyTorqueImpulse(motorImp);
tmp.negate(motorImp);
rbB.applyTorqueImpulse(tmp);
}
}
}
// shared code used by ODE solver
public void calculateTransforms() {
Transform tmpTrans = Stack.alloc(Transform.class);
if (useLinearReferenceFrameA) {
calculatedTransformA.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
calculatedTransformB.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
}
else {
calculatedTransformA.mul(rbB.getCenterOfMassTransform(tmpTrans), frameInB);
calculatedTransformB.mul(rbA.getCenterOfMassTransform(tmpTrans), frameInA);
}
realPivotAInW.set(calculatedTransformA.origin);
realPivotBInW.set(calculatedTransformB.origin);
calculatedTransformA.basis.getColumn(0, sliderAxis); // along X
delta.sub(realPivotBInW, realPivotAInW);
projPivotInW.scaleAdd(sliderAxis.dot(delta), sliderAxis, realPivotAInW);
Vector3f normalWorld = Stack.alloc(Vector3f.class);
// linear part
for (int i=0; i<3; i++) {
calculatedTransformA.basis.getColumn(i, normalWorld);
VectorUtil.setCoord(depth, i, delta.dot(normalWorld));
}
}
public void testLinLimits() {
solveLinLim = false;
linPos = depth.x;
if (lowerLinLimit <= upperLinLimit) {
if (depth.x > upperLinLimit) {
depth.x -= upperLinLimit;
solveLinLim = true;
}
else if (depth.x < lowerLinLimit) {
depth.x -= lowerLinLimit;
solveLinLim = true;
}
else {
depth.x = 0f;
}
}
else {
depth.x = 0f;
}
}
public void testAngLimits() {
angDepth = 0f;
solveAngLim = false;
if (lowerAngLimit <= upperAngLimit) {
Vector3f axisA0 = Stack.alloc(Vector3f.class);
calculatedTransformA.basis.getColumn(1, axisA0);
Vector3f axisA1 = Stack.alloc(Vector3f.class);
calculatedTransformA.basis.getColumn(2, axisA1);
Vector3f axisB0 = Stack.alloc(Vector3f.class);
calculatedTransformB.basis.getColumn(1, axisB0);
float rot = (float) Math.atan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
if (rot < lowerAngLimit) {
angDepth = rot - lowerAngLimit;
solveAngLim = true;
}
else if (rot > upperAngLimit) {
angDepth = rot - upperAngLimit;
solveAngLim = true;
}
}
}
// access for PE Solver
public Vector3f getAncorInA(Vector3f out) {
Transform tmpTrans = Stack.alloc(Transform.class);
Vector3f ancorInA = out;
ancorInA.scaleAdd((lowerLinLimit + upperLinLimit) * 0.5f, sliderAxis, realPivotAInW);
rbA.getCenterOfMassTransform(tmpTrans);
tmpTrans.inverse();
tmpTrans.transform(ancorInA);
return ancorInA;
}
public Vector3f getAncorInB(Vector3f out) {
Vector3f ancorInB = out;
ancorInB.set(frameInB.origin);
return ancorInB;
}
}