/*******************************************************************************
* Copyright (c) 2013, Daniel Murphy
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
package org.jbox2d.dynamics.joints;
import org.jbox2d.common.Mat22;
import org.jbox2d.common.Mat33;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Rot;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Vec2;
import org.jbox2d.common.Vec3;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.SolverData;
import org.jbox2d.pooling.IWorldPool;
//Linear constraint (point-to-line)
//d = p2 - p1 = x2 + r2 - x1 - r1
//C = dot(perp, d)
//Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
//J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
//
//Angular constraint
//C = a2 - a1 + a_initial
//Cdot = w2 - w1
//J = [0 0 -1 0 0 1]
//
//K = J * invM * JT
//
//J = [-a -s1 a s2]
// [0 -1 0 1]
//a = perp
//s1 = cross(d + r1, a) = cross(p2 - x1, a)
//s2 = cross(r2, a) = cross(p2 - x2, a)
//Motor/Limit linear constraint
//C = dot(ax1, d)
//Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
//J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
//Block Solver
//We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
//when the mass has poor distribution (leading to large torques about the joint anchor points).
//
//The Jacobian has 3 rows:
//J = [-uT -s1 uT s2] // linear
// [0 -1 0 1] // angular
// [-vT -a1 vT a2] // limit
//
//u = perp
//v = axis
//s1 = cross(d + r1, u), s2 = cross(r2, u)
//a1 = cross(d + r1, v), a2 = cross(r2, v)
//M * (v2 - v1) = JT * df
//J * v2 = bias
//
//v2 = v1 + invM * JT * df
//J * (v1 + invM * JT * df) = bias
//K * df = bias - J * v1 = -Cdot
//K = J * invM * JT
//Cdot = J * v1 - bias
//
//Now solve for f2.
//df = f2 - f1
//K * (f2 - f1) = -Cdot
//f2 = invK * (-Cdot) + f1
//
//Clamp accumulated limit impulse.
//lower: f2(3) = max(f2(3), 0)
//upper: f2(3) = min(f2(3), 0)
//
//Solve for correct f2(1:2)
//K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
//K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
//f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
//
//Now compute impulse to be applied:
//df = f2 - f1
/**
* A prismatic joint. This joint provides one degree of freedom: translation along an axis fixed in
* bodyA. Relative rotation is prevented. You can use a joint limit to restrict the range of motion
* and a joint motor to drive the motion or to model joint friction.
*
* @author Daniel
*/
public class PrismaticJoint extends Joint {
// Solver shared
protected final Vec2 m_localAnchorA;
protected final Vec2 m_localAnchorB;
protected final Vec2 m_localXAxisA;
protected final Vec2 m_localYAxisA;
protected float m_referenceAngle;
private final Vec3 m_impulse;
private float m_motorImpulse;
private float m_lowerTranslation;
private float m_upperTranslation;
private float m_maxMotorForce;
private float m_motorSpeed;
private boolean m_enableLimit;
private boolean m_enableMotor;
private LimitState m_limitState;
// Solver temp
private int m_indexA;
private int m_indexB;
private final Vec2 m_localCenterA = new Vec2();
private final Vec2 m_localCenterB = new Vec2();
private float m_invMassA;
private float m_invMassB;
private float m_invIA;
private float m_invIB;
private final Vec2 m_axis, m_perp;
private float m_s1, m_s2;
private float m_a1, m_a2;
private final Mat33 m_K;
private float m_motorMass; // effective mass for motor/limit translational constraint.
protected PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) {
super(argWorld, def);
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_localXAxisA = new Vec2(def.localAxisA);
m_localXAxisA.normalize();
m_localYAxisA = new Vec2();
Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse = new Vec3();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
m_K = new Mat33();
m_axis = new Vec2();
m_perp = new Vec2();
}
public Vec2 getLocalAnchorA() {
return m_localAnchorA;
}
public Vec2 getLocalAnchorB() {
return m_localAnchorB;
}
@Override
public void getAnchorA(Vec2 argOut) {
m_bodyA.getWorldPointToOut(m_localAnchorA, argOut);
}
@Override
public void getAnchorB(Vec2 argOut) {
m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
}
@Override
public void getReactionForce(float inv_dt, Vec2 argOut) {
Vec2 temp = pool.popVec2();
temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
argOut.set(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt);
pool.pushVec2(1);
}
@Override
public float getReactionTorque(float inv_dt) {
return inv_dt * m_impulse.y;
}
/**
* Get the current joint translation, usually in meters.
*/
public float getJointSpeed() {
Body bA = m_bodyA;
Body bB = m_bodyB;
Vec2 temp = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
Vec2 p1 = pool.popVec2();
Vec2 p2 = pool.popVec2();
Vec2 d = pool.popVec2();
Vec2 axis = pool.popVec2();
Vec2 temp2 = pool.popVec2();
Vec2 temp3 = pool.popVec2();
temp.set(m_localAnchorA).subLocal(bA.m_sweep.localCenter);
Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA);
temp.set(m_localAnchorB).subLocal(bB.m_sweep.localCenter);
Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB);
p1.set(bA.m_sweep.c).addLocal(rA);
p2.set(bB.m_sweep.c).addLocal(rB);
d.set(p2).subLocal(p1);
Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis);
Vec2 vA = bA.m_linearVelocity;
Vec2 vB = bB.m_linearVelocity;
float wA = bA.m_angularVelocity;
float wB = bB.m_angularVelocity;
Vec2.crossToOutUnsafe(wA, axis, temp);
Vec2.crossToOutUnsafe(wB, rB, temp2);
Vec2.crossToOutUnsafe(wA, rA, temp3);
temp2.addLocal(vB).subLocal(vA).subLocal(temp3);
float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2);
pool.pushVec2(9);
return speed;
}
public float getJointTranslation() {
Vec2 pA = pool.popVec2(), pB = pool.popVec2(), axis = pool.popVec2();
m_bodyA.getWorldPointToOut(m_localAnchorA, pA);
m_bodyB.getWorldPointToOut(m_localAnchorB, pB);
m_bodyA.getWorldVectorToOutUnsafe(m_localXAxisA, axis);
pB.subLocal(pA);
float translation = Vec2.dot(pB, axis);
pool.pushVec2(3);
return translation;
}
/**
* Is the joint limit enabled?
*
* @return
*/
public boolean isLimitEnabled() {
return m_enableLimit;
}
/**
* Enable/disable the joint limit.
*
* @param flag
*/
public void enableLimit(boolean flag) {
if (flag != m_enableLimit) {
m_bodyA.setAwake(true);
m_bodyB.setAwake(true);
m_enableLimit = flag;
m_impulse.z = 0.0f;
}
}
/**
* Get the lower joint limit, usually in meters.
*
* @return
*/
public float getLowerLimit() {
return m_lowerTranslation;
}
/**
* Get the upper joint limit, usually in meters.
*
* @return
*/
public float getUpperLimit() {
return m_upperTranslation;
}
/**
* Set the joint limits, usually in meters.
*
* @param lower
* @param upper
*/
public void setLimits(float lower, float upper) {
assert (lower <= upper);
if (lower != m_lowerTranslation || upper != m_upperTranslation) {
m_bodyA.setAwake(true);
m_bodyB.setAwake(true);
m_lowerTranslation = lower;
m_upperTranslation = upper;
m_impulse.z = 0.0f;
}
}
/**
* Is the joint motor enabled?
*
* @return
*/
public boolean isMotorEnabled() {
return m_enableMotor;
}
/**
* Enable/disable the joint motor.
*
* @param flag
*/
public void enableMotor(boolean flag) {
m_bodyA.setAwake(true);
m_bodyB.setAwake(true);
m_enableMotor = flag;
}
/**
* Set the motor speed, usually in meters per second.
*
* @param speed
*/
public void setMotorSpeed(float speed) {
m_bodyA.setAwake(true);
m_bodyB.setAwake(true);
m_motorSpeed = speed;
}
/**
* Get the motor speed, usually in meters per second.
*
* @return
*/
public float getMotorSpeed() {
return m_motorSpeed;
}
/**
* Set the maximum motor force, usually in N.
*
* @param force
*/
public void setMaxMotorForce(float force) {
m_bodyA.setAwake(true);
m_bodyB.setAwake(true);
m_maxMotorForce = force;
}
/**
* Get the current motor force, usually in N.
*
* @param inv_dt
* @return
*/
public float getMotorForce(float inv_dt) {
return m_motorImpulse * inv_dt;
}
public float getMaxMotorForce() {
return m_maxMotorForce;
}
public float getReferenceAngle() {
return m_referenceAngle;
}
public Vec2 getLocalAxisA() {
return m_localXAxisA;
}
@Override
public void initVelocityConstraints(final SolverData data) {
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final Vec2 d = pool.popVec2();
final Vec2 temp = pool.popVec2();
final Vec2 rA = pool.popVec2();
final Vec2 rB = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, d.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, d.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
temp.set(d).addLocal(rA);
m_a1 = Vec2.cross(temp, m_axis);
m_a2 = Vec2.cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f) {
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp);
temp.set(d).addLocal(rA);
m_s1 = Vec2.cross(temp, m_perp);
m_s2 = Vec2.cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f) {
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.set(k11, k12, k13);
m_K.ey.set(k12, k22, k23);
m_K.ez.set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit) {
float jointTranslation = Vec2.dot(m_axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) {
m_limitState = LimitState.EQUAL;
} else if (jointTranslation <= m_lowerTranslation) {
if (m_limitState != LimitState.AT_LOWER) {
m_limitState = LimitState.AT_LOWER;
m_impulse.z = 0.0f;
}
} else if (jointTranslation >= m_upperTranslation) {
if (m_limitState != LimitState.AT_UPPER) {
m_limitState = LimitState.AT_UPPER;
m_impulse.z = 0.0f;
}
} else {
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0f;
}
} else {
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false) {
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting) {
// Account for variable time step.
m_impulse.mulLocal(data.step.dtRatio);
m_motorImpulse *= data.step.dtRatio;
final Vec2 P = pool.popVec2();
temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp);
float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * LA;
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * LB;
pool.pushVec2(1);
} else {
m_impulse.setZero();
m_motorImpulse = 0.0f;
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushRot(2);
pool.pushVec2(4);
}
@Override
public void solveVelocityConstraints(final SolverData data) {
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
final Vec2 temp = pool.popVec2();
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != LimitState.EQUAL) {
temp.set(vB).subLocal(vA);
float Cdot = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA;
float impulse = m_motorMass * (m_motorSpeed - Cdot);
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorForce;
m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
final Vec2 P = pool.popVec2();
P.set(m_axis).mulLocal(impulse);
float LA = impulse * m_a1;
float LB = impulse * m_a2;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * LA;
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * LB;
pool.pushVec2(1);
}
final Vec2 Cdot1 = pool.popVec2();
temp.set(vB).subLocal(vA);
Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
// System.out.println(Cdot1);
if (m_enableLimit && m_limitState != LimitState.INACTIVE) {
// Solve prismatic and limit constraint in block form.
float Cdot2;
temp.set(vB).subLocal(vA);
Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA;
final Vec3 Cdot = pool.popVec3();
Cdot.set(Cdot1.x, Cdot1.y, Cdot2);
final Vec3 f1 = pool.popVec3();
final Vec3 df = pool.popVec3();
f1.set(m_impulse);
m_K.solve33ToOut(Cdot.negateLocal(), df);
// Cdot.negateLocal(); not used anymore
m_impulse.addLocal(df);
if (m_limitState == LimitState.AT_LOWER) {
m_impulse.z = MathUtils.max(m_impulse.z, 0.0f);
} else if (m_limitState == LimitState.AT_UPPER) {
m_impulse.z = MathUtils.min(m_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
// f1(1:2)
final Vec2 b = pool.popVec2();
final Vec2 f2r = pool.popVec2();
temp.set(m_K.ez.x, m_K.ez.y).mulLocal(m_impulse.z - f1.z);
b.set(Cdot1).negateLocal().subLocal(temp);
m_K.solve22ToOut(b, f2r);
f2r.addLocal(f1.x, f1.y);
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df.set(m_impulse).subLocal(f1);
final Vec2 P = pool.popVec2();
temp.set(m_axis).mulLocal(df.z);
P.set(m_perp).mulLocal(df.x).addLocal(temp);
float LA = df.x * m_s1 + df.y + df.z * m_a1;
float LB = df.x * m_s2 + df.y + df.z * m_a2;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * LA;
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * LB;
pool.pushVec2(3);
pool.pushVec3(3);
} else {
// Limit is inactive, just solve the prismatic constraint in block form.
final Vec2 df = pool.popVec2();
m_K.solve22ToOut(Cdot1.negateLocal(), df);
Cdot1.negateLocal();
m_impulse.x += df.x;
m_impulse.y += df.y;
final Vec2 P = pool.popVec2();
P.set(m_perp).mulLocal(df.x);
float LA = df.x * m_s1 + df.y;
float LB = df.x * m_s2 + df.y;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * LA;
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * LB;
pool.pushVec2(2);
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushVec2(2);
}
@Override
public boolean solvePositionConstraints(final SolverData data) {
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final Vec2 rA = pool.popVec2();
final Vec2 rB = pool.popVec2();
final Vec2 d = pool.popVec2();
final Vec2 axis = pool.popVec2();
final Vec2 perp = pool.popVec2();
final Vec2 temp = pool.popVec2();
final Vec2 C1 = pool.popVec2();
final Vec3 impulse = pool.popVec3();
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
qA.set(aA);
qB.set(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
Rot.mulToOutUnsafe(qA, m_localXAxisA, axis);
float a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
float a2 = Vec2.cross(rB, axis);
Rot.mulToOutUnsafe(qA, m_localYAxisA, perp);
float s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
float s2 = Vec2.cross(rB, perp);
C1.x = Vec2.dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
float linearError = MathUtils.abs(C1.x);
float angularError = MathUtils.abs(C1.y);
boolean active = false;
float C2 = 0.0f;
if (m_enableLimit) {
float translation = Vec2.dot(axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) {
// Prevent large angular corrections
C2 =
MathUtils.clamp(translation, -Settings.maxLinearCorrection,
Settings.maxLinearCorrection);
linearError = MathUtils.max(linearError, MathUtils.abs(translation));
active = true;
} else if (translation <= m_lowerTranslation) {
// Prevent large linear corrections and allow some slop.
C2 =
MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop,
-Settings.maxLinearCorrection, 0.0f);
linearError = MathUtils.max(linearError, m_lowerTranslation - translation);
active = true;
} else if (translation >= m_upperTranslation) {
// Prevent large linear corrections and allow some slop.
C2 =
MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0f,
Settings.maxLinearCorrection);
linearError = MathUtils.max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active) {
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f) {
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
final Mat33 K = pool.popMat33();
K.ex.set(k11, k12, k13);
K.ey.set(k12, k22, k23);
K.ez.set(k13, k23, k33);
final Vec3 C = pool.popVec3();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
K.solve33ToOut(C.negateLocal(), impulse);
pool.pushVec3(1);
pool.pushMat33(1);
} else {
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f) {
k22 = 1.0f;
}
final Mat22 K = pool.popMat22();
K.ex.set(k11, k12);
K.ey.set(k12, k22);
// temp is impulse1
K.solveToOut(C1.negateLocal(), temp);
C1.negateLocal();
impulse.x = temp.x;
impulse.y = temp.y;
impulse.z = 0.0f;
pool.pushMat22(1);
}
float Px = impulse.x * perp.x + impulse.z * axis.x;
float Py = impulse.x * perp.y + impulse.z * axis.y;
float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA.x -= mA * Px;
cA.y -= mA * Py;
aA -= iA * LA;
cB.x += mB * Px;
cB.y += mB * Py;
aB += iB * LB;
// data.positions[m_indexA].c.set(cA);
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c.set(cB);
data.positions[m_indexB].a = aB;
pool.pushVec2(7);
pool.pushVec3(1);
pool.pushRot(2);
return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop;
}
}