Vehicle::Vehicle():
m_carChassis(0)
{
rightIndex = 0;
upIndex = 1;
forwardIndex = 2;
gEngineForce = 0.f;
gBreakingForce = 0.f;
maxEngineForce = 100000.f; //1500.f; //this should be engine/velocity dependent
maxBreakingForce = 8000.f; // 100.f;
gVehicleSteering = 0.f;
steeringIncrement = 1.0f; //0.04f;
steeringClamp = 0.3f;
wheelRadius = 0.35f * PHYSCAR_SCALE; //** 0.5f;
wheelWidth = 0.2f * PHYSCAR_SCALE; //* 0.4f;
wheelFriction = 10; // 1000;//BT_LARGE_FLOAT;
suspensionStiffness = 50.f; // 20.f;
suspensionDamping = 0.3f * 2.0f * btSqrt(suspensionStiffness); // 2.3f;
suspensionCompression = 0.2f * 2.0f * btSqrt(suspensionStiffness); // 4.4f;
rollInfluence = 0.5f;
wheelDirectionCS0 = btVector3(0,-1,0);
wheelAxleCS = btVector3(-1,0,0);
suspensionRestLength = 0.15f * PHYSCAR_SCALE; //** (0.6);
m_vehicle = 0;
m_wheelShape = 0;
m_vehicleRayCaster = NULL;
m_vehicle = NULL;
m_compound = NULL;
}
static void M3x3getRot_ref(const btMatrix3x3 &m, btQuaternion &q)
{
btVector3 m_el[3] = {m[0], m[1], m[2]};
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
btScalar temp[4];
if (trace > btScalar(0.0))
{
btScalar s = btSqrt(trace + btScalar(1.0));
temp[3] = (s * btScalar(0.5));
s = btScalar(0.5) / s;
temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
}
else
{
int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
temp[i] = s * btScalar(0.5);
s = btScalar(0.5) / s;
temp[3] = (m_el[k][j] - m_el[j][k]) * s;
temp[j] = (m_el[j][i] + m_el[i][j]) * s;
temp[k] = (m_el[k][i] + m_el[i][k]) * s;
}
q.setValue(temp[0], temp[1], temp[2], temp[3]);
}
btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
int i;
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
for (i=0;i<getNumVertices();i++)
{
getVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
const btVector3& radius,
int res)
{
struct Hammersley
{
static void Generate(btVector3* x,int n)
{
for(int i=0;i<n;i++)
{
btScalar p=0.5,t=0;
for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
btScalar w=2*t-1;
btScalar a=(SIMD_PI+2*i*SIMD_PI)/n;
btScalar s=btSqrt(1-w*w);
*x++=btVector3(s*btCos(a),s*btSin(a),w);
}
}
};
btAlignedObjectArray<btVector3> vtx;
vtx.resize(3+res);
Hammersley::Generate(&vtx[0],vtx.size());
for(int i=0;i<vtx.size();++i)
{
vtx[i]=vtx[i]*radius+center;
}
return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
}
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
for (int i=0;i<m_unscaledPoints.size();i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v)
{
const int cylinderUpAxis = 1;
const int XX = 0;
const int YY = 1;
const int ZZ = 2;
btScalar radius = halfExtents[XX];
btScalar halfHeight = halfExtents[cylinderUpAxis];
btVector3 tmp;
btScalar d ;
btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
if (s != btScalar(0.0))
{
d = radius / s;
tmp[XX] = v[XX] * d;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = v[ZZ] * d;
return tmp;
}
else
{
tmp[XX] = radius;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = btScalar(0.0);
return tmp;
}
}
void EXPORT_API ProjectSphereCollisionConstraints(btVector3* predPosA, bool* posLocksA, int pointsCountA, float radiusA, float invMassA,
btVector3* predPosB, bool* posLocksB, int pointsCountB, float radiusB, float invMassB, float Ks_prime)
{
// float Ks_prime = 1.0f - Mathf.Pow((1.0f - Ks), 1.0f / solverIterations);
float radiusSum = radiusA + radiusB;
float radiusSumSq = radiusSum * radiusSum;
//#pragma omp parallel for
for (int idA = 0; idA < pointsCountA; idA++)
{
for (int idB = 0; idB < pointsCountB; idB++)
{
btVector3 dir = predPosA[idA] - predPosB[idB];
float distanceSq = dir.length2();
if (distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float distance = btSqrt(distanceSq);
btScalar w1 = posLocksA[idA] ? 0.0f : invMassA;
btScalar w2 = posLocksB[idB] ? 0.0f : invMassB;
float invMass = w1 + w2;
btVector3 dP = (1.0f / invMass) * (distance - radiusSum) * (dir / distance) * Ks_prime;
predPosA[idA] -= dP * w1;
predPosB[idB] += dP * w2;
}
}
}
btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
if( m_numPoints > 0 )
{
// Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically.
// btVector3 scaled = vec * m_localScaling;
int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints
return getScaledPoint(index);
}
return supVec;
}
void getInfo2 (btConstraintInfo2* info) {
btVector3 relA = m_rbA.getCenterOfMassTransform().getBasis() * getPivotInA();
btVector3 relB = m_rbB.getCenterOfMassTransform().getBasis() * getPivotInB();
btVector3 posA = m_rbA.getCenterOfMassTransform().getOrigin() + relA;
btVector3 posB = m_rbB.getCenterOfMassTransform().getOrigin() + relB;
btVector3 del = posB - posA;
btScalar currDist = btSqrt(del.dot(del));
btVector3 ortho = del / currDist;
info->m_J1linearAxis[0] = ortho[0];
info->m_J1linearAxis[1] = ortho[1];
info->m_J1linearAxis[2] = ortho[2];
btVector3 p, q;
p = relA.cross(ortho);
q = relB.cross(ortho);
info->m_J1angularAxis[0] = p[0];
info->m_J1angularAxis[1] = p[1];
info->m_J1angularAxis[2] = p[2];
info->m_J2angularAxis[0] = -q[0];
info->m_J2angularAxis[1] = -q[1];
info->m_J2angularAxis[2] = -q[2];
btScalar rhs = (currDist - m_dist) * info->fps * info->erp;
info->m_constraintError[0] = rhs;
info->cfm[0] = btScalar(0.f);
info->m_lowerLimit[0] = -SIMD_INFINITY;
info->m_upperLimit[0] = SIMD_INFINITY;
}
btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-1e30);
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
for (int i=0;i<m_numPoints;i++)
{
btVector3 vtx = getScaledPoint(i);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
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;
}
}
}
void EXPORT_API ComputeSphereCollisionsDEM(btVector3* positions, btVector3* velocities, btVector3* forces, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float radius, float spring, float damp, float shear, float attraction)
{
float radiusSum = radius + radius;
float radiusSumSq = radiusSum * radiusSum;
#pragma omp parallel
{
#pragma omp for
for (int idA = 0; idA < pointsCount; idA++)
{
btVector3 force(0, 0, 0);
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
// calculate relative position
btVector3 relPos = positions[idB] - positions[idA];
float distanceSq = relPos.length2();
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float dist = btSqrt(distanceSq);
//btScalar w1 = posLocks[idA] ? 0.0f : massInv;
//btScalar w2 = posLocks[idB] ? 0.0f : massInv;
float collideDist = radiusSum;
if (dist < collideDist)
{
btVector3 norm = relPos / dist;
// relative velocity
btVector3 relVel = velocities[idB] - velocities[idA];
// relative tangential velocity
btVector3 tanVel = relVel - (btDot(relVel, norm) * norm);
force += -spring*(collideDist - dist) * norm;
force += damp*relVel;
force += shear*tanVel;
force += attraction*relPos;
}
}
forces[idA] += force;
}
}
}
void CarHandlingDemo::addWheels(
btVector3* halfExtents,
btRaycastVehicle* vehicle,
btRaycastVehicle::btVehicleTuning tuning)
{
//The direction of the raycast, the btRaycastVehicle uses raycasts instead of simiulating the wheels with rigid bodies
btVector3 wheelDirectionCS0(0, -1, 0);
//The axis which the wheel rotates arround
btVector3 wheelAxleCS(-1, 0, 0);
btScalar suspensionRestLength(0.7);
btScalar wheelWidth(0.4);
btScalar wheelRadius(0.5);
//The height where the wheels are connected to the chassis
btScalar connectionHeight(1.2);
//All the wheel configuration assumes the vehicle is centered at the origin and a right handed coordinate system is used
btVector3 wheelConnectionPoint(halfExtents->x() - wheelRadius, connectionHeight, halfExtents->z() - wheelWidth);
//Adds the front wheels
vehicle->addWheel(wheelConnectionPoint, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, true);
vehicle->addWheel(wheelConnectionPoint * btVector3(-1, 1, 1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, true);
//Adds the rear wheels
vehicle->addWheel(wheelConnectionPoint* btVector3(1, 1, -1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, false);
vehicle->addWheel(wheelConnectionPoint * btVector3(-1, 1, -1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, false);
//Configures each wheel of our vehicle, setting its friction, damping compression, etc.
//For more details on what each parameter does, refer to the docs
for (int i = 0; i < vehicle->getNumWheels(); i++)
{
btWheelInfo& wheel = vehicle->getWheelInfo(i);
wheel.m_suspensionStiffness = 50;
wheel.m_wheelsDampingCompression = btScalar(0.3) * 2 * btSqrt(wheel.m_suspensionStiffness);//btScalar(0.8);
wheel.m_wheelsDampingRelaxation = btScalar(0.5) * 2 * btSqrt(wheel.m_suspensionStiffness);//1;
//Larger friction slips will result in better handling
wheel.m_frictionSlip = btScalar(1.2);
wheel.m_rollInfluence = 1;
}
}
void EXPORT_API ProjectInternalCollisionConstraintsMT(btVector3* predictedPositions, bool* posLocks, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float Ks_prime, float radius, float mass)
{
btScalar massInv = 1.0f / mass;
// float Ks_prime = 1.0f - Mathf.Pow((1.0f - Ks), 1.0f / solverIterations);
float radiusSum = radius + radius;
float radiusSumSq = radiusSum * radiusSum;
//#pragma omp parallel num_threads(3)
#pragma omp parallel
{
#pragma omp for
for (int idA = 0; idA < pointsCount; idA++)
{
btVector3 deltaP(0, 0, 0);
int collisionsCount = 0;
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
btVector3 dir = predictedPositions[idA] - predictedPositions[idB];
float distanceSq = dir.length2();
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float distance = btSqrt(distanceSq);
btScalar w1 = posLocks[idA] ? 0.0f : massInv;
btScalar w2 = posLocks[idB] ? 0.0f : massInv;
float invMass = w1 + w2;
btVector3 dP = (1.0f / invMass) * (distance - radiusSum) * (dir / distance) * Ks_prime;
deltaP -= dP * w1 / (btScalar)pointNeighboursCount[idA];
collisionsCount++;
// predictedPositions[idA] -= dP * w1;
// predictedPositions[idB] += dP * w2;
}
temp[idA] = deltaP;
//if (collisionsCount > 0)
// temp[idA] = deltaP / (btScalar)collisionsCount;
//else
// temp[idA] = btVector3(0, 0, 0);
}
#pragma omp for
for (int i = 0; i < pointsCount; i++)
{
predictedPositions[i] += temp[i];
}
}
}
btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (),
m_radius (radius),
m_height(height)
{
m_shapeType = CONE_SHAPE_PROXYTYPE;
setConeUpIndex(1);
btVector3 halfExtents;
m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
}
void btConeShape::setLocalScaling(const btVector3& scaling)
{
int axis = m_coneIndices[1];
int r1 = m_coneIndices[0];
int r2 = m_coneIndices[2];
m_height *= scaling[axis] / m_localScaling[axis];
m_radius *= (scaling[r1] / m_localScaling[r1] + scaling[r2] / m_localScaling[r2]) / 2;
m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
btConvexInternalShape::setLocalScaling(scaling);
}
/** Interpolates the normal at the given position for the triangle with
* a given index. The position must be inside of the given triangle.
* \param index Index of the triangle to use.
* \param position The position for which to interpolate the normal.
*/
btVector3 TriangleMesh::getInterpolatedNormal(unsigned int index,
const btVector3 &position) const
{
btVector3 p1, p2, p3;
getTriangle(index, &p1, &p2, &p3);
btVector3 n1, n2, n3;
getNormals(index, &n1, &n2, &n3);
// Compute the Barycentric coordinates of position inside triangle
// p1, p2, p3.
btVector3 edge1 = p2 - p1;
btVector3 edge2 = p3 - p1;
// Area of triangle ABC
btScalar p1p2p3 = edge1.cross(edge2).length2();
// Area of BCP
btScalar p2p3p = (p3 - p2).cross(position - p2).length2();
// Area of CAP
btScalar p3p1p = edge2.cross(position - p3).length2();
btScalar s = btSqrt(p2p3p / p1p2p3);
btScalar t = btSqrt(p3p1p / p1p2p3);
btScalar w = 1.0f - s - t;
#ifdef NORMAL_DEBUGGING
btVector3 regen_position = s * p1 + t * p2 + w * p3;
if((regen_position - position).length2() >= 0.0001f)
{
printf("bary:\n");
printf("new: %f %f %f\n", regen_position.getX(),regen_position.getY(),regen_position.getZ());
printf("old: %f %f %f\n", position.getX(), position.getY(),position.getZ());
printf("stw: %f %f %f\n", s, t, w);
printf("p1: %f %f %f\n", p1.getX(),p1.getY(),p1.getZ());
printf("p2: %f %f %f\n", p2.getX(),p2.getY(),p2.getZ());
printf("p3: %f %f %f\n", p3.getX(),p3.getY(),p3.getZ());
printf("pos: %f %f %f\n", position.getX(),position.getY(),position.getZ());
}
#endif
return s*n1 + t*n2 + w*n3;
} // getInterpolatedNormal
void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0,0,0));
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(m_dynamicsWorld->getDebugDrawer()->getDebugMode() + btIDebugDraw::DBG_DrawFrames);
btScalar sqr2 = btSqrt(2);
btVector3 tetraVerts[] = {
btVector3(1.f, 0.f, -1/sqr2),
btVector3(-1.f, 0.f, -1/sqr2),
btVector3(0, 1.f, 1/sqr2),
btVector3(0, -1.f, 1/sqr2),
};
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btCompoundShape* hull = new btCompoundShape();
btConvexHullShape* childHull = new btConvexHullShape(&tetraVerts[0].getX(),sizeof(tetraVerts)/sizeof(btVector3),sizeof(btVector3));
childHull->initializePolyhedralFeatures();
btTransform childTrans;
childTrans.setIdentity();
childTrans.setOrigin(btVector3(2,0,0));
hull->addChildShape(childTrans,childHull);
gfxBridge.createCollisionShapeGraphicsObject(hull);
m_collisionShapes.push_back(hull);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
hull->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(0,0,0));
btRigidBody* body = createRigidBody(mass,startTransform,hull);
gfxBridge.createRigidBodyGraphicsObject(body, btVector3(1, 1, 0));
}
}
btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
btScalar radius = getRadius();
{
btVector3 pos(0,0,0);
pos[getUpAxis()] = getHalfHeight();
vtx = pos +vec*(radius) - vec * getMargin();
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
{
btVector3 pos(0,0,0);
pos[getUpAxis()] = -getHalfHeight();
vtx = pos +vec*(radius) - vec * getMargin();
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
{
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
btVector3 gf = getAngularVelocity().cross(tmp);
btScalar l2 = gf.length2();
if (l2>maxGyroscopicForce*maxGyroscopicForce)
{
gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
}
return gf;
}
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(btScalar a,btScalar b, btScalar c)
{
btScalar radical = b * b - 4.0f * a * c;
if(radical >= 0.f)
{
btScalar sqrtRadical = btSqrt(radical);
btScalar idenom = 1.0f/(2.0f * a);
m_roots[0]=(-b + sqrtRadical) * idenom;
m_roots[1]=(-b - sqrtRadical) * idenom;
return 2;
}
return 0;
}
void btFluidSphSolverDefault::calculateSumsInCellSymmetric(const btFluidSphParametersGlobal& FG, int gridCellIndex,
const btFluidSortingGrid& grid, btFluidParticles& particles,
btFluidSphSolverDefault::SphParticles& sphData)
{
btFluidGridIterator currentCell = grid.getGridCell(gridCellIndex);
if(currentCell.m_firstIndex <= currentCell.m_lastIndex) //if cell is not empty
{
btFluidSortingGrid::FoundCells foundCells;
grid.findCellsSymmetric(particles.m_pos[currentCell.m_firstIndex], foundCells);
for(int i = currentCell.m_firstIndex; i <= currentCell.m_lastIndex; ++i)
{
//Remove particle, with index i, from grid cell to prevent self-particle interactions
++foundCells.m_iterators[0].m_firstIndex; //Local cell; currentCell == foundCells.m_iterators[0]
for(int cell = 0; cell < btFluidSortingGrid::NUM_FOUND_CELLS_SYMMETRIC; cell++)
{
btFluidGridIterator& FI = foundCells.m_iterators[cell];
for(int n = FI.m_firstIndex; n <= FI.m_lastIndex; ++n)
{
//Simulation-scale distance
btVector3 difference = (particles.m_pos[i] - particles.m_pos[n]) * FG.m_simulationScale;
btScalar distanceSquared = difference.length2();
if(FG.m_sphRadiusSquared > distanceSquared)
{
btScalar c = FG.m_sphRadiusSquared - distanceSquared;
btScalar poly6KernPartialResult = c * c * c;
sphData.m_invDensity[i] += poly6KernPartialResult;
sphData.m_invDensity[n] += poly6KernPartialResult;
btScalar distance = btSqrt(distanceSquared);
if( !sphData.m_neighborTable[i].isFilled() ) sphData.m_neighborTable[i].addNeighbor(n, distance);
else if( !sphData.m_neighborTable[n].isFilled() ) sphData.m_neighborTable[n].addNeighbor(i, distance);
else
{
cell = btFluidSortingGrid::NUM_FOUND_CELLS_SYMMETRIC; //Break out of outer loop
break;
}
}
}
}
}
}
}
btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
const btVector3* pos = &m_localPositionArray[0];
const btScalar* rad = &m_radiArray[0];
int numSpheres = m_localPositionArray.size();
for( int k = 0; k < numSpheres; k+= 128 )
{
btVector3 temp[128];
int inner_count = MIN( numSpheres - k, 128 );
for( long i = 0; i < inner_count; i++ )
{
temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
pos++;
rad++;
}
long i = vec.maxDot( temp, inner_count, newDot);
if( newDot > maxDot )
{
maxDot = newDot;
supVec = temp[i];
}
}
return supVec;
}
btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
int i;
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
const btVector3* pos = &m_localPositions[0];
const btScalar* rad = &m_radi[0];
for (i=0;i<m_numSpheres;i++)
{
vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
pos++;
rad++;
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
#ifndef __SPU__
int i;
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
for( int k = 0; k < getNumVertices(); k += 128 )
{
btVector3 temp[128];
int inner_count = MIN(getNumVertices() - k, 128);
for( i = 0; i < inner_count; i++ )
getVertex(i,temp[i]);
i = (int) vec.maxDot( temp, inner_count, newDot);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = temp[i];
}
}
#endif //__SPU__
return supVec;
}
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(btScalar p, btScalar q)
{
btScalar basic_h_local;
btScalar basic_h_local_delta;
basic_h_local = p * 0.5f;
basic_h_local_delta = basic_h_local * basic_h_local - q;
if (basic_h_local_delta > 0.0f) {
basic_h_local_delta = btSqrt(basic_h_local_delta);
m_roots[0] = - basic_h_local + basic_h_local_delta;
m_roots[1] = - basic_h_local - basic_h_local_delta;
return 2;
}
else if (btGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) {
m_roots[0] = - basic_h_local;
return 1;
}
else {
return 0;
}
}
btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
void EXPORT_API ProjectInternalCollisionConstraints(btVector3* predictedPositions, bool* posLocks, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float Ks_prime, float radius, float mass)
{
btScalar massInv = 1.0f / mass;
// float Ks_prime = 1.0f - Mathf.Pow((1.0f - Ks), 1.0f / solverIterations);
float radiusSum = radius + radius;
float radiusSumSq = radiusSum * radiusSum;
//#pragma omp parallel for
for (int idA = 0; idA < pointsCount; idA++)
{
for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
{
int idB = neighbours[idA * maxNeighboursPerPoint + nId];
btVector3 dir = predictedPositions[idA] - predictedPositions[idB];
float distanceSq = dir.length2();
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
continue;
float distance = btSqrt(distanceSq);
btScalar w1 = posLocks[idA] ? 0.0f : massInv;
btScalar w2 = posLocks[idB] ? 0.0f : massInv;
float invMass = w1 + w2;
btVector3 dP = (1.0f / invMass) * (distance - radiusSum) * (dir / distance) * Ks_prime;
predictedPositions[idA] -= dP * w1;
predictedPositions[idB] += dP * w2;
}
}
}
SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v)
{
const int cylinderUpAxis = 0;
const int XX = 1;
const int YY = 0;
const int ZZ = 2;
//mapping depends on how cylinder local orientation is
// extents of the cylinder is: X,Y is for radius, and Z for height
btScalar radius = halfExtents[XX];
btScalar halfHeight = halfExtents[cylinderUpAxis];
btVector3 tmp;
btScalar d ;
btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
if (s != btScalar(0.0))
{
d = radius / s;
tmp[XX] = v[XX] * d;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = v[ZZ] * d;
return tmp;
}
else
{
tmp[XX] = radius;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = btScalar(0.0);
return tmp;
}
}
void btRaycastVehicle::updateFriction(btScalar timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = getNumWheels();
if (!numWheel)
return;
m_forwardWS.resize(numWheel);
m_axle.resize(numWheel);
m_forwardImpulse.resize(numWheel);
m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i=0; i<getNumWheels(); i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.);
}
{
for (int i=0; i<getNumWheels(); i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
{
const btTransform& wheelTrans = getWheelTransformWS( i );
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
}
}
btScalar sideFactor = btScalar(1.);
btScalar fwdFactor = 0.5;
bool sliding = false;
{
for (int wheel =0; wheel <getNumWheels(); wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
btScalar rollingFriction = 0.f;
if (groundObject)
{
if (wheelInfo.m_engineForce != 0.f)
{
rollingFriction = wheelInfo.m_engineForce* timeStep;
} else
{
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = btScalar(0.);
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
if (groundObject)
{
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp;
btScalar maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
btScalar impulseSquared = (x*x + y*y);
if (impulseSquared > maximpSquared)
{
sliding = true;
btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < getNumWheels(); wheel++)
{
if (m_sideImpulse[wheel] != btScalar(0.))
{
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0; wheel<getNumWheels() ; wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.))
{
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
}
if (m_sideImpulse[wheel] != btScalar(0.))
{
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition();
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody->applyImpulse(sideImp,rel_pos);
//apply friction impulse on the ground
groundObject->applyImpulse(-sideImp,rel_pos2);
}
}
}
}
