forked from bartvdbraak/blender
325 lines
9.5 KiB
C++
325 lines
9.5 KiB
C++
|
/*
|
||
|
|
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
|
||
|
|
*
|
||
|
|
* Permission to use, copy, modify, distribute and sell this software
|
||
|
|
* and its documentation for any purpose is hereby granted without fee,
|
||
|
|
* provided that the above copyright notice appear in all copies.
|
||
|
|
* Erwin Coumans makes no representations about the suitability
|
||
|
|
* of this software for any purpose.
|
||
|
|
* It is provided "as is" without express or implied warranty.
|
||
|
|
*/
|
||
|
|
#include "ContactConstraint.h"
|
||
|
|
#include "Dynamics/RigidBody.h"
|
||
|
|
#include "SimdVector3.h"
|
||
|
|
#include "JacobianEntry.h"
|
||
|
|
#include "ContactSolverInfo.h"
|
||
|
|
#include "GEN_minmax.h"
|
||
|
|
|
||
|
|
#define ASSERT2 assert
|
||
|
|
|
||
|
|
|
||
|
|
static SimdScalar ContactThreshold = -10.0f;
|
||
|
|
|
||
|
|
float useGlobalSettingContacts = false;//true;
|
||
|
|
|
||
|
|
SimdScalar contactDamping = 0.9f;
|
||
|
|
SimdScalar contactTau = .2f;//0.02f;//*0.02f;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
|
||
|
|
{
|
||
|
|
return restitution;
|
||
|
|
// return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
|
||
|
|
}
|
||
|
|
|
||
|
|
void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
|
||
|
|
RigidBody& body2, const SimdVector3& pos2,
|
||
|
|
const SimdVector3& normal,float normalImpulse,
|
||
|
|
const ContactSolverInfo& solverInfo)
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
if (normalImpulse>0.f)
|
||
|
|
{
|
||
|
|
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||
|
|
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||
|
|
|
||
|
|
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||
|
|
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||
|
|
SimdVector3 vel = vel1 - vel2;
|
||
|
|
|
||
|
|
SimdScalar rel_vel = normal.dot(vel);
|
||
|
|
|
||
|
|
#define PER_CONTACT_FRICTION
|
||
|
|
#ifdef PER_CONTACT_FRICTION
|
||
|
|
SimdVector3 lat_vel = vel - normal * rel_vel;
|
||
|
|
SimdScalar lat_rel_vel = lat_vel.length();
|
||
|
|
|
||
|
|
if (lat_rel_vel > SIMD_EPSILON)
|
||
|
|
{
|
||
|
|
lat_vel /= lat_rel_vel;
|
||
|
|
SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
|
||
|
|
SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
|
||
|
|
SimdScalar frictionMaxImpulse = lat_rel_vel /
|
||
|
|
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||
|
|
SimdScalar frictionImpulse = (normalImpulse) * solverInfo.m_friction;
|
||
|
|
GEN_set_min(frictionImpulse,frictionMaxImpulse );
|
||
|
|
|
||
|
|
body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1);
|
||
|
|
body2.applyImpulse(lat_vel * frictionImpulse, rel_pos2);
|
||
|
|
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//bilateral constraint between two dynamic objects
|
||
|
|
void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
|
||
|
|
RigidBody& body2, const SimdVector3& pos2,
|
||
|
|
SimdScalar depth, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
|
||
|
|
{
|
||
|
|
float normalLenSqr = normal.length2();
|
||
|
|
ASSERT2(fabs(normalLenSqr) < 1.1f);
|
||
|
|
if (normalLenSqr > 1.1f)
|
||
|
|
{
|
||
|
|
impulse = 0.f;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||
|
|
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||
|
|
//this jacobian entry could be re-used for all iterations
|
||
|
|
|
||
|
|
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||
|
|
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||
|
|
SimdVector3 vel = vel1 - vel2;
|
||
|
|
|
||
|
|
SimdScalar rel_vel;
|
||
|
|
|
||
|
|
/*
|
||
|
|
|
||
|
|
JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||
|
|
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||
|
|
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
|
||
|
|
body2.getInvInertiaDiagLocal(),body2.getInvMass());
|
||
|
|
|
||
|
|
SimdScalar jacDiagAB = jac.getDiagonal();
|
||
|
|
SimdScalar jacDiagABInv = 1.f / jacDiagAB;
|
||
|
|
|
||
|
|
SimdScalar rel_vel = jac.getRelativeVelocity(
|
||
|
|
body1.getLinearVelocity(),
|
||
|
|
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
|
||
|
|
body2.getLinearVelocity(),
|
||
|
|
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
|
||
|
|
float a;
|
||
|
|
a=jacDiagABInv;
|
||
|
|
|
||
|
|
*/
|
||
|
|
rel_vel = normal.dot(vel);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/*int color = 255+255*256;
|
||
|
|
|
||
|
|
DrawRasterizerLine(pos1,pos1+normal,color);
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
|
||
|
|
|
||
|
|
float contactDamping = 0.9f;
|
||
|
|
impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv;
|
||
|
|
|
||
|
|
//SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
||
|
|
//impulse = velocityImpulse;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//velocity + friction
|
||
|
|
//response between two dynamic objects with friction
|
||
|
|
float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1,
|
||
|
|
RigidBody& body2, const SimdVector3& pos2,
|
||
|
|
SimdScalar depth, const SimdVector3& normal,
|
||
|
|
const ContactSolverInfo& solverInfo
|
||
|
|
)
|
||
|
|
{
|
||
|
|
|
||
|
|
float normalLenSqr = normal.length2();
|
||
|
|
ASSERT2(fabs(normalLenSqr) < 1.1f);
|
||
|
|
if (normalLenSqr > 1.1f)
|
||
|
|
return 0.f;
|
||
|
|
|
||
|
|
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||
|
|
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||
|
|
//this jacobian entry could be re-used for all iterations
|
||
|
|
|
||
|
|
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||
|
|
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||
|
|
SimdVector3 vel = vel1 - vel2;
|
||
|
|
SimdScalar rel_vel;
|
||
|
|
rel_vel = normal.dot(vel);
|
||
|
|
|
||
|
|
// if (rel_vel< 0.f)//-SIMD_EPSILON)
|
||
|
|
// {
|
||
|
|
SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution);
|
||
|
|
|
||
|
|
SimdScalar timeCorrection = 1.f;///timeStep;
|
||
|
|
float damping = solverInfo.m_damping ;
|
||
|
|
float tau = solverInfo.m_tau;
|
||
|
|
|
||
|
|
if (useGlobalSettingContacts)
|
||
|
|
{
|
||
|
|
damping = contactDamping;
|
||
|
|
tau = contactTau;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (depth < 0.f)
|
||
|
|
return 0.f;//bdepth = 0.f;
|
||
|
|
|
||
|
|
SimdScalar penetrationImpulse = (depth*tau*timeCorrection);// * massTerm;//jacDiagABInv
|
||
|
|
|
||
|
|
SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel;
|
||
|
|
|
||
|
|
SimdScalar impulse = penetrationImpulse + velocityImpulse;
|
||
|
|
SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(normal);
|
||
|
|
SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(normal);
|
||
|
|
impulse /=
|
||
|
|
(body1.getInvMass() + body2.getInvMass() + normal.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||
|
|
|
||
|
|
if (impulse > 0.f)
|
||
|
|
{
|
||
|
|
|
||
|
|
body1.applyImpulse(normal*(impulse), rel_pos1);
|
||
|
|
body2.applyImpulse(-normal*(impulse), rel_pos2);
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
impulse = 0.f;
|
||
|
|
}
|
||
|
|
|
||
|
|
return impulse;//velocityImpulse;//impulse;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//velocity + friction
|
||
|
|
//response between two dynamic objects with friction
|
||
|
|
float resolveSingleCollisionWithFriction(
|
||
|
|
|
||
|
|
RigidBody& body1,
|
||
|
|
const SimdVector3& pos1,
|
||
|
|
RigidBody& body2,
|
||
|
|
const SimdVector3& pos2,
|
||
|
|
SimdScalar depth,
|
||
|
|
const SimdVector3& normal,
|
||
|
|
|
||
|
|
const ContactSolverInfo& solverInfo
|
||
|
|
)
|
||
|
|
{
|
||
|
|
float normalLenSqr = normal.length2();
|
||
|
|
ASSERT2(fabs(normalLenSqr) < 1.1f);
|
||
|
|
if (normalLenSqr > 1.1f)
|
||
|
|
return 0.f;
|
||
|
|
|
||
|
|
SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||
|
|
SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||
|
|
//this jacobian entry could be re-used for all iterations
|
||
|
|
|
||
|
|
JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
|
||
|
|
body2.getCenterOfMassTransform().getBasis().transpose(),
|
||
|
|
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
|
||
|
|
body2.getInvInertiaDiagLocal(),body2.getInvMass());
|
||
|
|
|
||
|
|
SimdScalar jacDiagAB = jac.getDiagonal();
|
||
|
|
SimdScalar jacDiagABInv = 1.f / jacDiagAB;
|
||
|
|
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||
|
|
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||
|
|
SimdVector3 vel = vel1 - vel2;
|
||
|
|
SimdScalar rel_vel;
|
||
|
|
/* rel_vel = jac.getRelativeVelocity(
|
||
|
|
body1.getLinearVelocity(),
|
||
|
|
body1.getTransform().getBasis().transpose() * body1.getAngularVelocity(),
|
||
|
|
body2.getLinearVelocity(),
|
||
|
|
body2.getTransform().getBasis().transpose() * body2.getAngularVelocity());
|
||
|
|
*/
|
||
|
|
rel_vel = normal.dot(vel);
|
||
|
|
|
||
|
|
// if (rel_vel< 0.f)//-SIMD_EPSILON)
|
||
|
|
// {
|
||
|
|
SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution);
|
||
|
|
// SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
|
||
|
|
|
||
|
|
SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ;
|
||
|
|
|
||
|
|
float damping = solverInfo.m_damping ;
|
||
|
|
float tau = solverInfo.m_tau;
|
||
|
|
|
||
|
|
if (useGlobalSettingContacts)
|
||
|
|
{
|
||
|
|
damping = contactDamping;
|
||
|
|
tau = contactTau;
|
||
|
|
}
|
||
|
|
SimdScalar penetrationImpulse = (depth* tau *timeCorrection) * jacDiagABInv;
|
||
|
|
|
||
|
|
if (penetrationImpulse < 0.f)
|
||
|
|
penetrationImpulse = 0.f;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel * jacDiagABInv;
|
||
|
|
|
||
|
|
SimdScalar friction_impulse = 0.f;
|
||
|
|
|
||
|
|
if (velocityImpulse <= 0.f)
|
||
|
|
velocityImpulse = 0.f;
|
||
|
|
|
||
|
|
// SimdScalar impulse = penetrationImpulse + velocityImpulse;
|
||
|
|
//if (impulse > 0.f)
|
||
|
|
{
|
||
|
|
// SimdVector3 impulse_vector = normal * impulse;
|
||
|
|
body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1);
|
||
|
|
|
||
|
|
body2.applyImpulse(-normal*(velocityImpulse+penetrationImpulse), rel_pos2);
|
||
|
|
|
||
|
|
//friction
|
||
|
|
|
||
|
|
{
|
||
|
|
|
||
|
|
SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||
|
|
SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||
|
|
SimdVector3 vel = vel1 - vel2;
|
||
|
|
|
||
|
|
rel_vel = normal.dot(vel);
|
||
|
|
|
||
|
|
#define PER_CONTACT_FRICTION
|
||
|
|
#ifdef PER_CONTACT_FRICTION
|
||
|
|
SimdVector3 lat_vel = vel - normal * rel_vel;
|
||
|
|
SimdScalar lat_rel_vel = lat_vel.length();
|
||
|
|
|
||
|
|
if (lat_rel_vel > SIMD_EPSILON)
|
||
|
|
{
|
||
|
|
lat_vel /= lat_rel_vel;
|
||
|
|
SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
|
||
|
|
SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
|
||
|
|
friction_impulse = lat_rel_vel /
|
||
|
|
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||
|
|
SimdScalar normal_impulse = (penetrationImpulse+
|
||
|
|
velocityImpulse) * solverInfo.m_friction;
|
||
|
|
GEN_set_min(friction_impulse, normal_impulse);
|
||
|
|
|
||
|
|
body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
|
||
|
|
body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
|
||
|
|
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return velocityImpulse + friction_impulse;
|
||
|
|
}
|