btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
{
btScalar halfHeight = m_height * btScalar(0.5);
if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
{
btVector3 tmp;
tmp[m_coneIndices[0]] = btScalar(0.);
tmp[m_coneIndices[1]] = halfHeight;
tmp[m_coneIndices[2]] = btScalar(0.);
return tmp;
}
else {
btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
if (s > SIMD_EPSILON) {
btScalar d = m_radius / s;
btVector3 tmp;
tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
tmp[m_coneIndices[1]] = -halfHeight;
tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
return tmp;
}
else {
btVector3 tmp;
tmp[m_coneIndices[0]] = btScalar(0.);
tmp[m_coneIndices[1]] = -halfHeight;
tmp[m_coneIndices[2]] = btScalar(0.);
return tmp;
}
}
}
float du_get_body_speed(du_body_id body)
{
btRigidBody *bt_body = reinterpret_cast <btRigidBody*>(body);
const btVector3 velocity = bt_body->getLinearVelocity();
float speed_ms = velocity.length();
float speed = speed_ms *= 3.6;
return speed;
}
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 pressure */
inline float VRFluids::kernel_spiky(btVector3 v, float h) {
float r = v.length();
float diff = h - r;
if (diff > 0) {
return (15.0 / (Pi * pow(h,6))) * diff*diff*diff;
} else {
return 0.0;
}
}
/**
* Kernel_visc laplacian function for viscosity
* Source: mueller
*/
inline float VRFluids::kernel_visc_laplacian(btVector3 v, float h) {
float r = v.length();
if (r <= h && r >= 0) {
float diff = h - r;
return (45.0 / (Pi * pow(h,6))) * diff;
} else {
return 0.0;
}
}
/** Kernel for pressure */
inline btVector3 VRFluids::kernel_spiky_gradient(btVector3 v, float h) {
float r = v.length();
float diff = h - r;
if (diff > 0 && r > 0) { // NOTE > should be >= but things won't work then for some reason
return (-45.0 / (Pi * pow(h,6))) * (v/r) * diff*diff;
} else {
return btVector3(0,0,0);
}
}
// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
btVector3 n(0, 0, 0);
if (v.length() > SIMD_EPSILON) {
n = v.normalized();
}
return n;
}
btVector3 EntityPhysic::colVelocity(btVector3 velocity_)
{
int MAX=120;
int MIN=50;
if(velocity_.length()>MAX)
{
float radio=velocity_.length()/MAX;
velocity_=btVector3(velocity_.getX()/radio,
velocity_.getY()/radio,
velocity_.getZ()/radio);
}
if(velocity_.length()<MIN)
{
float radio=velocity_.length()/MIN;
velocity_=btVector3(velocity_.getX()/radio,
velocity_.getY()/radio,
velocity_.getZ()/radio);
}
return velocity_;
}
/** Kernel for viscosity */
inline float VRFluids::kernel_visc(btVector3 v, float h) {
float r = v.length();
if (r <= h) {
float h2 = h*h;
float h3 = h2*h;
float r2 = r*r;
float r3 = r2*r;
float a = -1*r3 / (h3+h3);
float b = r2 / h2;
float c = h / (r+r);
return (15.0 / (2*Pi*h3)) * (a + b + c - 1);
} else {
return 0.0;
}
}
/**
* Returns the unit vector in the direction of this spring.
*/
const btVector3 tgBulletCompressionSpring::getAnchorDirectionUnitVector() const
{
// Get the unit vector for the direction of the force, this is needed for
// applying the force to the rigid bodies.
const btVector3 dist =
anchor2->getWorldPosition() - anchor1->getWorldPosition();
// The unit vector of the direction of the force will be needed later
// In order to have a positive force move the two rigid bodies away
// from each other, this unit vector must be in the opposite direction
// of this calculation. Otherwise, a positive force brings them closer
// together. Needs a minus.
// note that dist.length is a scalar double.
const btVector3 unitVector = - dist / dist.length();
return unitVector;
}
btMatrix3x3 GetOrientedBasis(btVector3 const &z)
{
btAssert(fabsf(z.length()-1) < 0.01f);
btVector3 t(0,0,0);
if(fabsf(z.z() < 0.999f))
{
t.setZ(1);
}
else
{
t.setX(1);
}
btVector3 x = t.cross(z).normalize();
btVector3 y = z.cross(x).normalize();
return btMatrix3x3( x.x(), y.x(), z.x(),
x.y(), y.y(), z.y(),
x.z(), y.z(), z.z());
}
void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
{
//start with static aabb
getAabb(curTrans,temporalAabbMin,temporalAabbMax);
btScalar temporalAabbMaxx = temporalAabbMax.getX();
btScalar temporalAabbMaxy = temporalAabbMax.getY();
btScalar temporalAabbMaxz = temporalAabbMax.getZ();
btScalar temporalAabbMinx = temporalAabbMin.getX();
btScalar temporalAabbMiny = temporalAabbMin.getY();
btScalar temporalAabbMinz = temporalAabbMin.getZ();
// add linear motion
btVector3 linMotion = linvel*timeStep;
///@todo: simd would have a vector max/min operation, instead of per-element access
if (linMotion.x() > btScalar(0.))
temporalAabbMaxx += linMotion.x();
else
temporalAabbMinx += linMotion.x();
if (linMotion.y() > btScalar(0.))
temporalAabbMaxy += linMotion.y();
else
temporalAabbMiny += linMotion.y();
if (linMotion.z() > btScalar(0.))
temporalAabbMaxz += linMotion.z();
else
temporalAabbMinz += linMotion.z();
//add conservative angular motion
btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
temporalAabbMin -= angularMotion3d;
temporalAabbMax += angularMotion3d;
}
void AIInput::step(JeepActor* jeep, btVector3 const & pathDir1, btVector3 const & pathDir2, btVector3 const & trackVector, btVector3 const & segmentVec1, btVector3 const & segmentVec2) {
btVector3 trackDirection = pathDir1;
btVector3 actorHeading = quatRotate(jeep->orientation, btVector3(1,0,0)).normalize();
AcceleratePressed = true;
float angleToTrack = trackDirection.dot(actorHeading);
float turnDir = trackDirection.cross(actorHeading).getY();
if (turnDir > 0) turnDir = 1;
else if (turnDir < 0) turnDir = -1;
else turnDir = 0;
int parallelize = 0;
float parallelizationThreshold = LoadFloat("config/ai.xml","parallelization_threshold");
float hardParallelizationThreshold = LoadFloat("config/ai.xml","hard_parallelization_threshold");
if ((1.0 - angleToTrack) * turnDir > hardParallelizationThreshold) parallelize = 2;
if ((1.0 - angleToTrack) * turnDir > parallelizationThreshold) parallelize = 1;
else if ((1.0 - angleToTrack) * turnDir < -hardParallelizationThreshold) parallelize = -2;
else if ((1.0 - angleToTrack) * turnDir < -parallelizationThreshold) parallelize = -1;
int onTrack = 0;
btVector3 pathVec = trackVector;
float distFromTrack = pathVec.length();
turnDir = trackDirection.cross(pathVec).getY();
if (turnDir > 0) turnDir = 1;
else if (turnDir < 0) turnDir = -1;
else turnDir = 0;
float distanceThreshold = LoadFloat("config/ai.xml","distance_threshold");
float farDistanceThreshold = LoadFloat("config/ai.xml","far_distance_threshold");
if (distFromTrack > farDistanceThreshold && turnDir > 0) onTrack = 2;
if (distFromTrack > distanceThreshold && turnDir > 0) onTrack = 1;
else if (distFromTrack > farDistanceThreshold && turnDir < 0) onTrack = -2;
else if (distFromTrack > distanceThreshold && turnDir < 0) onTrack = -1;
//std::cout << distFromTrack << std::endl;
float distToNextSeg = segmentVec1.length();
float distToNextSeg2 = segmentVec2.length();
int turnAnticipation1 = 0;
int turnAnticipation2 = 0;
//std::cout << distToNextSeg << "\t" << distToNextSeg2 << std::endl;
float turnAnticipationThreshold = LoadFloat("config/ai.xml","turn_anticipation_threshold");
float farTurnAnticipationThreshold = LoadFloat("config/ai.xml","far_turn_anticipation_threshold");
if (distToNextSeg < turnAnticipationThreshold) {
trackDirection = pathDir1;
angleToTrack = trackDirection.dot(actorHeading);
turnDir = trackDirection.cross(actorHeading).getY();
if (turnDir > 0) turnDir = 1;
else if (turnDir < 0) turnDir = -1;
else turnDir = 0;
if ((1.0 - angleToTrack) * turnDir > 0.2) turnAnticipation1 = 2;
else if ((1.0 - angleToTrack) * turnDir < -0.2) turnAnticipation1 = -2;
}
if (distToNextSeg2 < farTurnAnticipationThreshold) {
trackDirection = pathDir2;
angleToTrack = trackDirection.dot(actorHeading);
turnDir = trackDirection.cross(actorHeading).getY();
if (turnDir > 0) turnDir = 1;
else if (turnDir < 0) turnDir = -1;
else turnDir = 0;
if ((1.0 - angleToTrack) * turnDir > 0.2) turnAnticipation2 = 1;
else if ((1.0 - angleToTrack) * turnDir < -0.2) turnAnticipation2 = -1;
}
// std::cout << parallelize << " " << onTrack << " " << turnAnticipation1 << " " << turnAnticipation2 << std::endl;
XAxis = parallelize + onTrack + turnAnticipation1 + turnAnticipation2;
if (XAxis > 0) XAxis = 1;
else if (XAxis < 0) XAxis = -1;
else XAxis = 0;
}
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
(void)linvelA;
(void)linvelB;
(void)angvelB;
(void)angvelA;
imp0 = btScalar(0.);
imp1 = btScalar(0.);
btScalar len = btFabs(normalA.length()) - btScalar(1.);
if (btFabs(len) >= SIMD_EPSILON)
return;
btAssert(len < SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
// calculate rhs (or error) terms
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
void collisionWithBound(btVector3 &vel, btVector3 &pos, const btVector3 &normal, const btScalar &penetrationDist)
{
if( penetrationDist < btScalar(0.0) )
{
pos = pos - normal * penetrationDist;
vel = vel - (1 + Constant.m_boundaryRestitution * (-penetrationDist) / (Constant.m_timeStep * vel.length())) * vel.dot(normal) * normal;
}
}
//-----------------------------------------------------------------------
// s t e p F o r w a r d A n d S t r a f e
//-----------------------------------------------------------------------
void TKinematicCharacterTest::stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove)
{
btVector3 originalDir = walkMove.normalized();
if (walkMove.length() < SIMD_EPSILON)
{
originalDir.setValue(0.f,0.f,0.f);
}
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe
btTransform start, end;
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
end.setIdentity ();
btScalar fraction = 1.0;
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
if (m_touchingContact)
{
if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
updateTargetPositionBasedOnCollision (m_touchingNormal);
}
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
{
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
TKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest)
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
}
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
if (callback.hasHit())
{
if(callback.m_hitCollisionObject->isStaticObject())
{
// we moved only a fraction
btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
if (hitDistance<0.f)
{
// printf("neg dist?\n");
}
/* If the distance is farther than the collision margin, move */
if (hitDistance > m_addedMargin)
{
// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
}
updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
btVector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.length2();
if (distance2 > SIMD_EPSILON)
{
currentDir.normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
if (currentDir.dot(originalDir) <= btScalar(0.0))
{
break;
}
} else
{
// printf("currentDir: don't normalize a zero vector\n");
break;
}
}
} else {
// we moved whole way
m_currentPosition = m_targetPosition;
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}
/**
* Returns the distance between the two anchors.
* This is similar to the getActualLength function in tgBulletSpringCable.
*/
const double tgBulletCompressionSpring::getCurrentAnchorDistance() const
{
const btVector3 dist =
this->anchor2->getWorldPosition() - this->anchor1->getWorldPosition();
return dist.length();
}