/*
* JBox2D - A Java Port of Erin Catto's Box2D
*
* JBox2D homepage: http://jbox2d.sourceforge.net/
* Box2D homepage: http://www.box2d.org
*
* 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.
*/
package org.jbox2d.dynamics.joints;
import org.jbox2d.common.Mat22;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Vec2;
import org.jbox2d.common.XForm;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.TimeStep;
import org.jbox2d.pooling.TLMat22;
import org.jbox2d.pooling.TLVec2;
//Updated to rev 56->130 of b2MouseJoint.cpp/.h
//p = attached point, m = mouse point
//C = p - m
//Cdot = v
// = v + cross(w, r)
//J = [I r_skew]
//Identity used:
//w k % (rx i + ry j) = w * (-ry i + rx j)
//
public class MouseJoint extends Joint {
public final Vec2 m_localAnchor;
public final Vec2 m_target;
public final Vec2 m_force;
public final Mat22 m_mass; // effective mass for point-to-point constraint.
public final Vec2 m_C; // position error
public float m_maxForce;
public float m_beta; // bias factor
public float m_gamma; // softness
public MouseJoint(final MouseJointDef def) {
super(def);
m_force = new Vec2();
m_target = new Vec2();
m_C = new Vec2();
m_mass = new Mat22();
m_target.set(def.target);
m_localAnchor = XForm.mulTrans(m_body2.m_xf, m_target);
m_maxForce = def.maxForce;
final float mass = m_body2.m_mass;
// Frequency
final float omega = 2.0f * Settings.pi * def.frequencyHz;
// Damping coefficient
final float d = 2.0f * mass * def.dampingRatio * omega;
// Spring stiffness
final float k = mass * omega * omega;
// magic formulas
m_gamma = 1.0f / (d + def.timeStep * k);
m_beta = def.timeStep * k / (d + def.timeStep * k);
}
/** Use this to update the target point. */
public void setTarget(final Vec2 target) {
if (m_body2.isSleeping()) {
m_body2.wakeUp();
}
m_target.set(target);
}
@Override
public Vec2 getAnchor1() {
return m_target;
}
// djm pooled
private static final TLVec2 tlanchor2 = new TLVec2();
/**
* this comes from a pooled value
*/
@Override
public Vec2 getAnchor2() {
final Vec2 anchor2 = tlanchor2.get();
m_body2.getWorldLocationToOut(m_localAnchor, anchor2);
return anchor2;
}
// djm pooled
private static final TLVec2 tlr = new TLVec2();
private static final TLMat22 tlK1 = new TLMat22();
private static final TLMat22 tlK2 = new TLMat22();
@Override
public void initVelocityConstraints(final TimeStep step) {
final Body b = m_body2;
final Vec2 r = tlr.get();
final Mat22 K1 = tlK1.get();
final Mat22 K2 = tlK2.get();
// Compute the effective mass matrix.
r.set(m_localAnchor);
r.subLocal(b.getMemberLocalCenter());
Mat22.mulToOut(b.m_xf.R, r, r);
// Vec2 r = Mat22.mul(b.m_xf.R,
// m_localAnchor.sub(b.getMemberLocalCenter()));
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2)
// * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 *
// [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
final float invMass = b.m_invMass;
final float invI = b.m_invI;
K1.set(invMass, 0.0f, 0.0f, invMass);
K2.set(invI * r.y * r.y, -invI * r.x * r.y, -invI * r.x * r.y, invI
* r.x * r.x);
// Mat22 K = K1.add(K2);
K1.addLocal(K2);
K1.col1.x += m_gamma;
K1.col2.y += m_gamma;
K1.invertToOut( m_mass);
m_C.set(b.m_sweep.c.x + r.x - m_target.x, b.m_sweep.c.y + r.y
- m_target.y);
// Cheat with some damping
b.m_angularVelocity *= 0.98f;
// Warm starting.
final float Px = step.dt * m_force.x;
final float Py = step.dt * m_force.y;
b.m_linearVelocity.x += invMass * Px;
b.m_linearVelocity.y += invMass * Py;
b.m_angularVelocity += invI * (r.x * Py - r.y * Px);
}
@Override
public boolean solvePositionConstraints() {
return true;
}
// djm pooled, from above too
private static final TLVec2 tlCdot = new TLVec2();
private static final TLVec2 tlforce = new TLVec2();
private static final TLVec2 tloldForce = new TLVec2();
private static final TLVec2 tlP = new TLVec2();
@Override
public void solveVelocityConstraints(final TimeStep step) {
final Body b = m_body2;
final Vec2 r = tlr.get();
final Vec2 Cdot = tlCdot.get();
final Vec2 force = tlforce.get();
final Vec2 oldForce = tloldForce.get();
final Vec2 P = tlP.get();
r.set(m_localAnchor);
r.subLocal(b.getMemberLocalCenter());
Mat22.mulToOut(b.m_xf.R, r, r);
// Vec2 r = Mat22.mul(b.m_xf.R,
// m_localAnchor.sub(b.getMemberLocalCenter()));
// Cdot = v + cross(w, r)
Vec2.crossToOut(b.m_angularVelocity, r, Cdot);
Cdot.addLocal(b.m_linearVelocity);
//Vec2 Cdot = b.m_linearVelocity.add(Vec2.cross(b.m_angularVelocity, r));
// Vec2 force = -step.inv_dt * Mat22.mul(m_mass, Cdot + (m_beta *
// step.inv_dt) * m_C + m_gamma * step.dt * m_force);
/*Vec2 force = new Vec2(Cdot.x + (m_beta * step.inv_dt) * m_C.x + m_gamma
* step.dt * m_force.x, Cdot.y + (m_beta * step.inv_dt) * m_C.y
+ m_gamma * step.dt * m_force.y);*/
force.set(Cdot.x + (m_beta * step.inv_dt) * m_C.x + m_gamma
* step.dt * m_force.x, Cdot.y + (m_beta * step.inv_dt) * m_C.y
+ m_gamma * step.dt * m_force.y);
Mat22.mulToOut(m_mass, force, force);
force.mulLocal(-step.inv_dt);
oldForce.set(m_force);
m_force.addLocal(force);
final float forceMagnitude = m_force.length();
if (forceMagnitude > m_maxForce) {
m_force.mulLocal(m_maxForce / forceMagnitude);
}
force.set(m_force.x - oldForce.x, m_force.y - oldForce.y);
P.x = step.dt * force.x;
P.y = step.dt * force.y;
b.m_angularVelocity += b.m_invI * Vec2.cross(r, P);
b.m_linearVelocity.addLocal(P.mulLocal(b.m_invMass));
}
@Override
public Vec2 getReactionForce() {
return m_force;
}
@Override
public float getReactionTorque() {
return 0.0f;
}
}