/*
|
* 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.
|
*/
|
|
/*
|
2007-09-09
|
btGeneric6DofConstraint Refactored by Francisco Le�n
|
email: projectileman@yahoo.com
|
http://gimpact.sf.net
|
*/
|
|
package com.bulletphysics.dynamics.constraintsolver;
|
|
import com.bulletphysics.BulletGlobals;
|
import com.bulletphysics.dynamics.RigidBody;
|
import cz.advel.stack.Stack;
|
import cz.advel.stack.StaticAlloc;
|
import javax.vecmath.Vector3f;
|
|
/**
|
* Rotation limit structure for generic joints.
|
*
|
* @author jezek2
|
*/
|
public class RotationalLimitMotor implements java.io.Serializable {
|
|
//protected final BulletStack stack = BulletStack.get();
|
|
public float loLimit; //!< joint limit
|
public float hiLimit; //!< joint limit
|
public float targetVelocity; //!< target motor velocity
|
public float maxMotorForce; //!< max force on motor
|
public float maxLimitForce; //!< max force on limit
|
public float damping; //!< Damping.
|
public float limitSoftness; //! Relaxation factor
|
public float ERP; //!< Error tolerance factor when joint is at limit
|
public float bounce; //!< restitution factor
|
public boolean enableMotor;
|
|
public float currentLimitError;//! How much is violated this limit
|
public int currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
|
public float accumulatedImpulse;
|
|
public RotationalLimitMotor() {
|
accumulatedImpulse = 0.f;
|
targetVelocity = 0;
|
maxMotorForce = 0.1f;
|
maxLimitForce = 300.0f;
|
loLimit = -BulletGlobals.SIMD_INFINITY;
|
hiLimit = BulletGlobals.SIMD_INFINITY;
|
ERP = 0.5f;
|
bounce = 0.0f;
|
damping = 1.0f;
|
limitSoftness = 0.5f;
|
currentLimit = 0;
|
currentLimitError = 0;
|
enableMotor = false;
|
}
|
|
public RotationalLimitMotor(RotationalLimitMotor limot) {
|
targetVelocity = limot.targetVelocity;
|
maxMotorForce = limot.maxMotorForce;
|
limitSoftness = limot.limitSoftness;
|
loLimit = limot.loLimit;
|
hiLimit = limot.hiLimit;
|
ERP = limot.ERP;
|
bounce = limot.bounce;
|
currentLimit = limot.currentLimit;
|
currentLimitError = limot.currentLimitError;
|
enableMotor = limot.enableMotor;
|
}
|
|
/**
|
* Is limited?
|
*/
|
public boolean isLimited()
|
{
|
if(loLimit>=hiLimit) return false;
|
return true;
|
}
|
|
/**
|
* Need apply correction?
|
*/
|
public boolean needApplyTorques()
|
{
|
if(currentLimit == 0 && enableMotor == false) return false;
|
return true;
|
}
|
|
/**
|
* Calculates error. Calculates currentLimit and currentLimitError.
|
*/
|
public int testLimitValue(float test_value) {
|
if (loLimit > hiLimit) {
|
currentLimit = 0; // Free from violation
|
return 0;
|
}
|
|
if (test_value < loLimit) {
|
currentLimit = 1; // low limit violation
|
currentLimitError = test_value - loLimit;
|
return 1;
|
}
|
else if (test_value > hiLimit) {
|
currentLimit = 2; // High limit violation
|
currentLimitError = test_value - hiLimit;
|
return 2;
|
}
|
|
currentLimit = 0; // Free from violation
|
return 0;
|
}
|
|
/**
|
* Apply the correction impulses for two bodies.
|
*/
|
@StaticAlloc
|
public float solveAngularLimits(float timeStep, Vector3f axis, float jacDiagABInv, RigidBody body0, RigidBody body1) {
|
if (needApplyTorques() == false) {
|
return 0.0f;
|
}
|
|
float target_velocity = this.targetVelocity;
|
float maxMotorForce = this.maxMotorForce;
|
|
// current error correction
|
if (currentLimit != 0) {
|
target_velocity = -ERP * currentLimitError / (timeStep);
|
maxMotorForce = maxLimitForce;
|
}
|
|
maxMotorForce *= timeStep;
|
|
// current velocity difference
|
Vector3f vel_diff = body0.getAngularVelocity(Stack.alloc(Vector3f.class));
|
if (body1 != null) {
|
vel_diff.sub(body1.getAngularVelocity(Stack.alloc(Vector3f.class)));
|
}
|
|
float rel_vel = axis.dot(vel_diff);
|
|
// correction velocity
|
float motor_relvel = limitSoftness * (target_velocity - damping * rel_vel);
|
|
if (motor_relvel < BulletGlobals.FLT_EPSILON && motor_relvel > -BulletGlobals.FLT_EPSILON) {
|
return 0.0f; // no need for applying force
|
}
|
|
// correction impulse
|
float unclippedMotorImpulse = (1 + bounce) * motor_relvel * jacDiagABInv;
|
|
// clip correction impulse
|
float clippedMotorImpulse;
|
|
// todo: should clip against accumulated impulse
|
if (unclippedMotorImpulse > 0.0f) {
|
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
|
}
|
else {
|
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
|
}
|
|
// sort with accumulated impulses
|
float lo = -1e30f;
|
float hi = 1e30f;
|
|
float oldaccumImpulse = accumulatedImpulse;
|
float sum = oldaccumImpulse + clippedMotorImpulse;
|
accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
|
|
clippedMotorImpulse = accumulatedImpulse - oldaccumImpulse;
|
|
Vector3f motorImp = Stack.alloc(Vector3f.class);
|
motorImp.scale(clippedMotorImpulse, axis);
|
|
body0.applyTorqueImpulse(motorImp);
|
if (body1 != null) {
|
motorImp.negate();
|
body1.applyTorqueImpulse(motorImp);
|
}
|
|
return clippedMotorImpulse;
|
}
|
|
}
|
