void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
velocity = btVector3(0.0f, 0.0f, 0.0f);
}
// measure velocity changes and their weights
std::vector<btVector3> velocities;
velocities.reserve(_motors.size());
std::vector<btScalar> weights;
weights.reserve(_motors.size());
for (int i = 0; i < (int)_motors.size(); ++i) {
applyMotor(i, dt, velocity, velocities, weights);
}
assert(velocities.size() == weights.size());
// blend velocity changes according to relative weights
btScalar totalWeight = 0.0f;
for (size_t i = 0; i < weights.size(); ++i) {
totalWeight += weights[i];
}
if (totalWeight > 0.0f) {
velocity = btVector3(0.0f, 0.0f, 0.0f);
for (size_t i = 0; i < velocities.size(); ++i) {
velocity += (weights[i] / totalWeight) * velocities[i];
}
}
if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
velocity = btVector3(0.0f, 0.0f, 0.0f);
}
// 'thrust' is applied at the very end
velocity += dt * _linearAcceleration;
_targetVelocity = velocity;
}
bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance )
{
const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
btScalar boxMargin = boxShape->getMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
btTransform const &m44T = boxObjWrap->getWorldTransform();
btVector3 sphereRelPos = m44T.invXform(sphereCenter);
// Determine the closest point to the sphere center in the box
btVector3 closestPoint = sphereRelPos;
closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
btScalar intersectionDist = fRadius + boxMargin;
btScalar contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
btScalar dist2 = normal.length2();
if (dist2 > contactDist * contactDist)
{
return false;
}
btScalar distance;
//special case if the sphere center is inside the box
if (dist2 == 0.0f)
{
distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
}
else //compute the penetration details
{
distance = normal.length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
btVector3 tmp = m44T(pointOnBox);
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T.getBasis() * normal;
normal = tmp;
return true;
}
/** Kernel for density (poly6)
* @inproceedings{muller2003particle,
* title={Particle-based fluid simulation for interactive applications},
* author={M{\"u}ller, Matthias and Charypar, David and Gross, Markus},
* booktitle={Proceedings of the 2003 ACM SIGGRAPH/Eurographics symposium on Computer animation},
* pages={154--159},
* year={2003},
* organization={Eurographics Association}
* }
*/
inline float VRFluids::kernel_poly6(btVector3 v, float h) {
float r2 = v.length2();
float h2 = h*h;
if ( r2 <= h2 ) {
float diff = h2 - r2;
return (315.0 / (64.0*Pi)) * (1/pow(h,9)) * diff*diff*diff;
} else {
return 0.0;
}
}
//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
(void)timeStep;
(void)distance;
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
{
impulse = btScalar(0.);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = btScalar(0.2);
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
bool EpaFace::Initialize()
{
assert( m_pHalfEdge && "Must setup half-edge first!" );
CollectVertices( m_pVertices );
const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const btScalar e0Sqrd = e0.length2();
const btScalar e1Sqrd = e1.length2();
const btScalar e0e1 = e0.dot( e1 );
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
if ( IsAffinelyDependent() )
{
return false;
}
CalcClosestPoint();
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( !CalculatePlane() )
{
return false;
}
#endif
return true;
}
//bilateral constraint between two dynamic objects
void RaycastCar::resolveSingleBilateral(btRigidBody & body1,
const btVector3 & pos1,
btRigidBody & body2,
const btVector3 & pos2,
const btVector3 & normal,
btScalar & impulse)
{
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1f));
if (normalLenSqr > btScalar(1.1f))
{
impulse = btScalar(0.0f);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,
rel_pos2,
normal,
body1.getInvInertiaDiagLocal(),
body1.getInvMass(),
body2.getInvInertiaDiagLocal(),
body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.0f) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity
(body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose()*body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose()*body2.getAngularVelocity());
btScalar velocityImpulse = -1.0f * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}