maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
godot/servers/physics/joints/generic_6dof_joint_sw.cpp
Juan Linietsky 8cab401d08 3D Physics Rework, Other Stuff 
-=-=-=-=-=-=-=-=-=-=-=-=-=-

3D Physics:
-Fixed "Bounce" parameter in 3D
-Fixed bug affecting Area (sometims it would not detect properly)
-Vehicle Body has seen heavy work
-Added Query API for doing space queries in 3D. Needs some docs though.
-Added JOINTS! Adapted Bullet Joints: and created easy gizmos for setting them up:
   -PinJoint
   -HingeJoint (with motor)
   -SliderJoint
   -ConeTwistJoint
   -Generic6DOFJoint
-Added OBJECT PICKING! based on the new query API. Any physics object now (Area or Body) has the following signals and virtual functions:
    -input_event (mouse or multitouch input over the body)
    -mouse_enter (mouse entered the body area)
    -mouse_exit (mouse exited body area)
   For Area it needs to be activated manually, as it isn't by default (ray goes thru).

Other:

-Begun working on Windows 8 (RT) port. Compiles but does not work yet.
-Added TheoraPlayer library for improved to-texture and portable video support.
-Fixed a few bugs in the renderer, collada importer, collada exporter, etc.
2014-09-15 11:33:30 -03:00

692 lines
17 KiB
C++
Raw Blame History

#include "generic_6dof_joint_sw.h"
#define GENERIC_D6_DISABLE_WARMSTARTING 1
real_t btGetMatrixElem(const Matrix3& mat, int index);
real_t btGetMatrixElem(const Matrix3& mat, int index)
{
int i = index%3;
int j = index/3;
return mat[i][j];
}
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz);
bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz)
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
if (btGetMatrixElem(mat,2) < real_t(1.0))
{
if (btGetMatrixElem(mat,2) > real_t(-1.0))
{
xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = Math::asin(btGetMatrixElem(mat,2));
xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = -Math_PI*0.5;
xyz[2] = real_t(0.0);
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = Math_PI*0.5;
xyz[2] = 0.0;
}
return false;
}
//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value)
{
if(m_loLimit>m_hiLimit)
{
m_currentLimit = 0;//Free from violation
return 0;
}
if (test_value < m_loLimit)
{
m_currentLimit = 1;//low limit violation
m_currentLimitError = test_value - m_loLimit;
return 1;
}
else if (test_value> m_hiLimit)
{
m_currentLimit = 2;//High limit violation
m_currentLimitError = test_value - m_hiLimit;
return 2;
};
m_currentLimit = 0;//Free from violation
return 0;
}
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
real_t timeStep,Vector3& axis,real_t jacDiagABInv,
BodySW * body0, BodySW * body1)
{
if (needApplyTorques()==false) return 0.0f;
real_t target_velocity = m_targetVelocity;
real_t maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
Vector3 vel_diff = body0->get_angular_velocity();
if (body1)
{
vel_diff -= body1->get_angular_velocity();
}
real_t rel_vel = axis.dot(vel_diff);
// correction velocity
real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
real_t 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
real_t lo = real_t(-1e30);
real_t hi = real_t(1e30);
real_t oldaccumImpulse = m_accumulatedImpulse;
real_t sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
Vector3 motorImp = clippedMotorImpulse * axis;
body0->apply_torque_impulse(motorImp);
if (body1) body1->apply_torque_impulse(-motorImp);
return clippedMotorImpulse;
}
//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
real_t timeStep,
real_t jacDiagABInv,
BodySW* body1,const Vector3 &pointInA,
BodySW* body2,const Vector3 &pointInB,
int limit_index,
const Vector3 & axis_normal_on_a,
const Vector3 & anchorPos)
{
///find relative velocity
// Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
// Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
Vector3 vel = vel1 - vel2;
real_t rel_vel = axis_normal_on_a.dot(vel);
/// apply displacement correction
//positional error (zeroth order error)
real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
real_t lo = real_t(-1e30);
real_t hi = real_t(1e30);
real_t minLimit = m_lowerLimit[limit_index];
real_t maxLimit = m_upperLimit[limit_index];
//handle the limits
if (minLimit < maxLimit)
{
{
if (depth > maxLimit)
{
depth -= maxLimit;
lo = real_t(0.);
}
else
{
if (depth < minLimit)
{
depth -= minLimit;
hi = real_t(0.);
}
else
{
return 0.0f;
}
}
}
}
real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;
real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
real_t sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1->apply_impulse( rel_pos1, impulse_vector);
body2->apply_impulse( rel_pos2, -impulse_vector);
return normalImpulse;
}
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
: JointSW(_arr,2)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA)
{
A=rbA;
B=rbB;
A->add_constraint(this,0);
B->add_constraint(this,1);
}
void Generic6DOFJointSW::calculateAngleInfo()
{
Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt = 0 ax[1]
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
// to prove the result for angle[0], write the expression for angle[0] from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
m_calculatedAxis[1] = axis2.cross(axis0);
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
// if(m_debugDrawer)
// {
//
// char buff[300];
// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
// m_calculatedAxisAngleDiff[0],
// m_calculatedAxisAngleDiff[1],
// m_calculatedAxisAngleDiff[2]);
// m_debugDrawer->reportErrorWarning(buff);
// }
}
void Generic6DOFJointSW::calculateTransforms()
{
m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB;
calculateAngleInfo();
}
void Generic6DOFJointSW::buildLinearJacobian(
JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
const Vector3 & pivotAInW,const Vector3 & pivotBInW)
{
memnew_placement(&jacLinear, JacobianEntrySW(
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
pivotAInW - A->get_transform().origin,
pivotBInW - B->get_transform().origin,
normalWorld,
A->get_inv_inertia(),
A->get_inv_mass(),
B->get_inv_inertia(),
B->get_inv_mass()));
}
void Generic6DOFJointSW::buildAngularJacobian(
JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
{
memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
A->get_transform().basis.transposed(),
B->get_transform().basis.transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
}
bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
{
real_t angle = m_calculatedAxisAngleDiff[axis_index];
//test limits
m_angularLimits[axis_index].testLimitValue(angle);
return m_angularLimits[axis_index].needApplyTorques();
}
bool Generic6DOFJointSW::setup(float p_step) {
// Clear accumulated impulses for the next simulation step
m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
int i;
for(i = 0; i < 3; i++)
{
m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
}
//calculates transform
calculateTransforms();
// const Vector3& pivotAInW = m_calculatedTransformA.origin;
// const Vector3& pivotBInW = m_calculatedTransformB.origin;
calcAnchorPos();
Vector3 pivotAInW = m_AnchorPos;
Vector3 pivotBInW = m_AnchorPos;
// not used here
// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
Vector3 normalWorld;
//linear part
for (i=0;i<3;i++)
{
if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
{
if (m_useLinearReferenceFrameA)
normalWorld = m_calculatedTransformA.basis.get_axis(i);
else
normalWorld = m_calculatedTransformB.basis.get_axis(i);
buildLinearJacobian(
m_jacLinear[i],normalWorld ,
pivotAInW,pivotBInW);
}
}
// angular part
for (i=0;i<3;i++)
{
//calculates error angle
if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
{
normalWorld = this->getAxis(i);
// Create angular atom
buildAngularJacobian(m_jacAng[i],normalWorld);
}
}
return true;
}
void Generic6DOFJointSW::solve(real_t timeStep)
{
m_timeStep = timeStep;
//calculateTransforms();
int i;
// linear
Vector3 pointInA = m_calculatedTransformA.origin;
Vector3 pointInB = m_calculatedTransformB.origin;
real_t jacDiagABInv;
Vector3 linear_axis;
for (i=0;i<3;i++)
{
if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
{
jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
if (m_useLinearReferenceFrameA)
linear_axis = m_calculatedTransformA.basis.get_axis(i);
else
linear_axis = m_calculatedTransformB.basis.get_axis(i);
m_linearLimits.solveLinearAxis(
m_timeStep,
jacDiagABInv,
A,pointInA,
B,pointInB,
i,linear_axis, m_AnchorPos);
}
}
// angular
Vector3 angular_axis;
real_t angularJacDiagABInv;
for (i=0;i<3;i++)
{
if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
{
// get axis
angular_axis = getAxis(i);
angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B);
}
}
}
void Generic6DOFJointSW::updateRHS(real_t timeStep)
{
(void)timeStep;
}
Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
{
return m_calculatedAxis[axis_index];
}
real_t Generic6DOFJointSW::getAngle(int axis_index) const
{
return m_calculatedAxisAngleDiff[axis_index];
}
void Generic6DOFJointSW::calcAnchorPos(void)
{
real_t imA = A->get_inv_mass();
real_t imB = B->get_inv_mass();
real_t weight;
if(imB == real_t(0.0))
{
weight = real_t(1.0);
}
else
{
weight = imA / (imA + imB);
}
const Vector3& pA = m_calculatedTransformA.origin;
const Vector3& pB = m_calculatedTransformB.origin;
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
return;
} // Generic6DOFJointSW::calcAnchorPos()
void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
ERR_FAIL_INDEX(p_axis,3);
switch(p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
m_linearLimits.m_lowerLimit[p_axis]=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
m_linearLimits.m_upperLimit[p_axis]=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
m_linearLimits.m_limitSoftness[p_axis]=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
m_linearLimits.m_restitution[p_axis]=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
m_linearLimits.m_damping[p_axis]=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
m_angularLimits[p_axis].m_loLimit=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
m_angularLimits[p_axis].m_hiLimit=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
m_angularLimits[p_axis].m_limitSoftness;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
m_angularLimits[p_axis].m_damping=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
m_angularLimits[p_axis].m_bounce=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
m_angularLimits[p_axis].m_maxLimitForce=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
m_angularLimits[p_axis].m_ERP=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
m_angularLimits[p_axis].m_targetVelocity=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
m_angularLimits[p_axis].m_maxLimitForce=p_value;
} break;
}
}
float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
ERR_FAIL_INDEX_V(p_axis,3,0);
switch(p_param) {
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
return m_linearLimits.m_lowerLimit[p_axis];
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
return m_linearLimits.m_upperLimit[p_axis];
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
return m_linearLimits.m_limitSoftness[p_axis];
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
return m_linearLimits.m_restitution[p_axis];
} break;
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
return m_linearLimits.m_damping[p_axis];
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
return m_angularLimits[p_axis].m_loLimit;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
return m_angularLimits[p_axis].m_hiLimit;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
return m_angularLimits[p_axis].m_limitSoftness;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
return m_angularLimits[p_axis].m_damping;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
return m_angularLimits[p_axis].m_bounce;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
return m_angularLimits[p_axis].m_maxLimitForce;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
return m_angularLimits[p_axis].m_ERP;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
return m_angularLimits[p_axis].m_targetVelocity;
} break;
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
return m_angularLimits[p_axis].m_maxLimitForce;
} break;
}
return 0;
}
void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
ERR_FAIL_INDEX(p_axis,3);
switch(p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
m_linearLimits.enable_limit[p_axis]=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
m_angularLimits[p_axis].m_enableLimit=p_value;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
m_angularLimits[p_axis].m_enableMotor=p_value;
} break;
}
}
bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
ERR_FAIL_INDEX_V(p_axis,3,0);
switch(p_flag) {
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
return m_linearLimits.enable_limit[p_axis];
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
return m_angularLimits[p_axis].m_enableLimit;
} break;
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
return m_angularLimits[p_axis].m_enableMotor;
} break;
}
return 0;
}
Reference in a new issue View git blame Copy permalink