PxQuat slerp(const PxReal t, const PxQuat& left, const PxQuat& right)
{
const PxReal quatEpsilon = (PxReal(1.0e-8f));
PxReal cosine = left.dot(right);
PxReal sign = PxReal(1);
if (cosine < 0)
{
cosine = -cosine;
sign = PxReal(-1);
}
PxReal sine = PxReal(1) - cosine*cosine;
if(sine>=quatEpsilon*quatEpsilon)
{
sine = PxSqrt(sine);
const PxReal angle = PxAtan2(sine, cosine);
const PxReal i_sin_angle = PxReal(1) / sine;
const PxReal leftw = PxSin(angle*(PxReal(1)-t)) * i_sin_angle;
const PxReal rightw = PxSin(angle * t) * i_sin_angle * sign;
return left * leftw + right * rightw;
}
return left;
}
void integrateTransform(const PxTransform& curTrans, const PxVec3& linvel, const PxVec3& angvel, PxReal timeStep, PxTransform& result)
{
result.p = curTrans.p + linvel * timeStep;
//from void PxsDynamicsContext::integrateAtomPose(PxsRigidBody* atom, Cm::BitMap &shapeChangedMap) const:
// Integrate the rotation using closed form quaternion integrator
PxReal w = angvel.magnitudeSquared();
if(w != 0.0f)
{
w = PxSqrt(w);
if (w != 0.0f)
{
const PxReal v = timeStep * w * 0.5f;
const PxReal q = PxCos(v);
const PxReal s = PxSin(v) / w;
const PxVec3 pqr = angvel * s;
const PxQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
PxQuat out; //need to have temporary, otherwise we may overwrite input if &curTrans == &result.
out = quatVel * curTrans.q;
out.x += curTrans.q.x * q;
out.y += curTrans.q.y * q;
out.z += curTrans.q.z * q;
out.w += curTrans.q.w * q;
result.q = out;
return;
}
}
//orientation stays the same - convert from quat to matrix:
result.q = curTrans.q;
}
void Camera3D::setFrustum()
{
PxReal tang = tanf(m_fov * 0.5f);
PxReal nearHt = m_near * tang;
PxReal nearWd = nearHt * m_aspect;
PxReal farHt = m_far * tang;
PxReal farWd = farHt * m_aspect;
//Our view of the game is backward which is why we are rotating 180
PxVec3 viewDirection = Utility::getForward(getTransform()->getTransformedRot() * PxQuat(PxSin(0.5f * ToRadians(180.0f)), 0, 0, PxCos(0.5f * ToRadians(180.0f))));
PxVec3 rightDirection = Utility::getRight(getTransform()->getTransformedRot() * PxQuat(PxSin(0.5f * ToRadians(180.0f)), 0, 0, PxCos(0.5f * ToRadians(180.0f))));
PxVec3 upDirection = Utility::getUp(getTransform()->getTransformedRot() * PxQuat(PxSin(0.5f * ToRadians(180.0f)), 0, 0, PxCos(0.5f * ToRadians(180.0f))));
PxVec3 nearCentre = getTransform()->getTransformedPos() - (viewDirection * m_near);
PxVec3 farCentre = getTransform()->getTransformedPos() - (viewDirection * m_far);
PxVec3 ntl = nearCentre + upDirection * nearHt - rightDirection * nearWd;
PxVec3 ntr = nearCentre + upDirection * nearHt + rightDirection * nearWd;
PxVec3 nbl = nearCentre - upDirection * nearHt - rightDirection * nearWd;
PxVec3 nbr = nearCentre - upDirection * nearHt + rightDirection * nearWd;
PxVec3 ftl = farCentre + upDirection * farHt - rightDirection * farWd;
PxVec3 ftr = farCentre + upDirection * farHt + rightDirection * farWd;
PxVec3 fbl = farCentre - upDirection * farHt - rightDirection * farWd;
PxVec3 fbr = farCentre - upDirection * farHt + rightDirection * farWd;
m_frustum[0] = PxPlane(ntl, ntr, nbr);
m_frustum[1] = PxPlane(ftr, ftl, fbl);
m_frustum[2] = PxPlane(ntr, ntl, ftl);
m_frustum[3] = PxPlane(nbl, nbr, fbr);
m_frustum[4] = PxPlane(ntl, nbl, fbl);
m_frustum[5] = PxPlane(nbr, ntr, fbr);
}
void PxFlyBall::move()
{
time++;
if (time % 3000 < 1600) {
PxVec3 v = pxActor->getLinearVelocity();
if (fabs(v.x)< 0.0001 && fabs(v.z)<0.0001) {
GLfloat angle = (rand() % 360) / 360.0*PxPi;
pxActor->addForce(PxVec3(PxSin(angle) * 50, 0, PxCos(angle) * 50), PxForceMode::eACCELERATION);
}
else {
if (fabs(v.x) < 50 && fabs(v.z) < 50) {
PxVec3 nmvec = v.getNormalized();
pxActor->addForce(PxVec3(nmvec.x*50.0, 0, nmvec.z*50.0), PxForceMode::eACCELERATION);
}
}
PxTransform t = pxActor->getGlobalPose();
if(t.p.y<300)
pxActor->addForce(PxVec3(0, rand()%200, 0.0), PxForceMode::eACCELERATION);
if (particleTime == 0) {
addCPSS();
}
}
//particleTime++;
//particleTime = particleTime > 5 ? 0 : particleTime;
time = time > 3000 ? 0 : time;
}
void ParticleEmitter::computeSiteVelocity(PxVec3& siteVel, const PxVec3& sitePos)
{
//velocity dir noise
PxReal noiseXYAngle = randInRange(0.0f, PxTwoPi);
PxReal noiseZAngle = randInRange(0.0f, mRandomAngle);
PxVec3 noiseDirVec = mAxisX * PxCos(noiseXYAngle) + mAxisY * PxSin(noiseXYAngle);
noiseDirVec.normalize();
noiseDirVec = mAxisZ * PxCos(noiseZAngle) + noiseDirVec * PxSin(noiseZAngle);
siteVel = noiseDirVec * mVelocity;
//add emitter repulsion
if (mParticleMass > 0.0f)
{
mLinMomentum -= siteVel;
mAngMomentum -= (sitePos - mBodyCenter).cross(siteVel);
}
if (mFrameBody)
siteVel += mBodyLinVel + (mBodyAngVel.cross(sitePos - mBodyCenter));
}
void PxMoveBall::move()
{
PxVec3 v = pxActor->getLinearVelocity();
if (fabs(v.x - 0.0)<0.0001 && fabs(v.z-0.0)<0.0001) {
GLfloat angle = (rand() % 360) / 360.0*PxPi;
pxActor->addForce(PxVec3(PxSin(angle)*50, 0, PxCos(angle)*50),PxForceMode::eACCELERATION);
}
else {
PxReal speed = v.magnitude();
if (speed <= maxspeed) {
PxVec3 nmvec = v.getNormalized();
pxActor->addForce(PxVec3(nmvec.x*50.0, 0, nmvec.z*50.0),PxForceMode::eACCELERATION);
}
}
}
PxConvexMesh* Vehicle::createWheelMesh(const PxF32 width, const PxF32 radius, PxPhysics& physics, PxCooking& cooking)
{
PxVec3 points[2 * 16];
for (PxU32 i = 0; i < 16; i++)
{
const PxF32 cosTheta = PxCos(i*PxPi*2.0f / 16.0f);
const PxF32 sinTheta = PxSin(i*PxPi*2.0f / 16.0f);
const PxF32 y = radius*cosTheta;
const PxF32 z = radius*sinTheta;
points[2 * i + 0] = PxVec3(-width / 2.0f, y, z);
points[2 * i + 1] = PxVec3(+width / 2.0f, y, z);
}
return createConvexMesh(points, 32, physics, cooking);
}
PxConvexMesh* createCylinderConvexMesh(const PxF32 width, const PxF32 radius, const PxU32 numCirclePoints, PxPhysics& physics, PxCooking& cooking)
{
#define MAX_NUM_VERTS_IN_CIRCLE 16
PX_ASSERT(numCirclePoints<MAX_NUM_VERTS_IN_CIRCLE);
PxVec3 verts[2*MAX_NUM_VERTS_IN_CIRCLE];
PxU32 numVerts=2*numCirclePoints;
const PxF32 dtheta=2*PxPi/(1.0f*numCirclePoints);
for(PxU32 i=0;i<MAX_NUM_VERTS_IN_CIRCLE;i++)
{
const PxF32 theta=dtheta*i;
const PxF32 cosTheta=radius*PxCos(theta);
const PxF32 sinTheta=radius*PxSin(theta);
verts[2*i+0]=PxVec3(-0.5f*width, cosTheta, sinTheta);
verts[2*i+1]=PxVec3(+0.5f*width, cosTheta, sinTheta);
}
return createConvexMesh(verts,numVerts,physics,cooking);
}