PX_FORCE_INLINE void RTreePage::growChildBounds(PxU32 index, const RTreeNodeQ& child)
{
PX_ASSERT(index < RTreePage::SIZE);
minx[index] = PxMin(minx[index], child.minx);
miny[index] = PxMin(miny[index], child.miny);
minz[index] = PxMin(minz[index], child.minz);
maxx[index] = PxMax(maxx[index], child.maxx);
maxy[index] = PxMax(maxy[index], child.maxy);
maxz[index] = PxMax(maxz[index], child.maxz);
}
PX_FORCE_INLINE void RTreeNodeQ::grow(const RTreePage& page, int nodeIndex)
{
PX_ASSERT(nodeIndex < RTreePage::SIZE);
minx = PxMin(minx, page.minx[nodeIndex]);
miny = PxMin(miny, page.miny[nodeIndex]);
minz = PxMin(minz, page.minz[nodeIndex]);
maxx = PxMax(maxx, page.maxx[nodeIndex]);
maxy = PxMax(maxy, page.maxy[nodeIndex]);
maxz = PxMax(maxz, page.maxz[nodeIndex]);
}
//process value in range(-1,1)
PX_FORCE_INLINE PxF32 processAnalogValue
(const PxF32 riseRate, const PxF32 fallRate,
const PxF32 currentVal, const PxF32 targetVal,
const PxF32 timestep)
{
PX_ASSERT(PxAbs(targetVal)<=1.01f);
PxF32 val=0.0f; // PT: the following code could leave that variable uninitialized!!!!!
if(0==targetVal)
{
//Drift slowly back to zero
if(currentVal>0)
{
val=currentVal-fallRate*timestep;
val=PxMax(val,0.0f);
}
else if(currentVal<0)
{
val=currentVal+fallRate*timestep;
val=PxMin(val,0.0f);
}
}
else
{
if(currentVal < targetVal)
{
if(currentVal<0)
{
val=currentVal + fallRate*timestep;
val=PxMin(val,targetVal);
}
else
{
val=currentVal + riseRate*timestep;
val=PxMin(val,targetVal);
}
}
else
{
if(currentVal>0)
{
val=currentVal - fallRate*timestep;
val=PxMax(val,targetVal);
}
else
{
val=currentVal - riseRate*timestep;
val=PxMax(val,targetVal);
}
}
}
return val;
}
PxsMaterialCombiner::PxsCombinedMaterial PxsMaterialCombiner::combineIsotropicFriction(const PxsMaterialData& mat0, const PxsMaterialData& mat1)
{
PxsCombinedMaterial dest;
dest.flags = (mat0.flags | mat1.flags); //& (PxMaterialFlag::eDISABLE_STRONG_FRICTION|PxMaterialFlag::eDISABLE_FRICTION); //eventually set DisStrongFric flag, lower all others.
if (!(dest.flags & PxMaterialFlag::eDISABLE_FRICTION))
{
const PxI32 fictionCombineMode = PxMax(mat0.getFrictionCombineMode(), mat1.getFrictionCombineMode());
PxReal dynFriction = 0.f;
PxReal staFriction = 0.f;
switch (fictionCombineMode)
{
case PxCombineMode::eAVERAGE:
dynFriction = 0.5f * (mat0.dynamicFriction + mat1.dynamicFriction);
staFriction = 0.5f * (mat0.staticFriction + mat1.staticFriction);
break;
case PxCombineMode::eMIN:
dynFriction = PxMin(mat0.dynamicFriction, mat1.dynamicFriction);
staFriction = PxMin(mat0.staticFriction, mat1.staticFriction);
break;
case PxCombineMode::eMULTIPLY:
dynFriction = (mat0.dynamicFriction * mat1.dynamicFriction);
staFriction = (mat0.staticFriction * mat1.staticFriction);
break;
case PxCombineMode::eMAX:
dynFriction = PxMax(mat0.dynamicFriction, mat1.dynamicFriction);
staFriction = PxMax(mat0.staticFriction, mat1.staticFriction);
break;
}
dynFriction*=mDynamicFrictionScaling;
staFriction*=mStaticFrictionScaling;
//isotropic case
const PxReal fDynFriction = PxMax(dynFriction, 0.f);
const PxReal fStaFriction = physx::intrinsics::fsel(staFriction - fDynFriction, staFriction, fDynFriction);
dest.dynFriction = fDynFriction;
dest.staFriction = fStaFriction;
}
else
{
dest.flags |= PxMaterialFlag::eDISABLE_STRONG_FRICTION;
dest.staFriction = 0.0f;
dest.dynFriction = 0.0f;
}
return dest;
}
void SampleVehicleWayPoints::update(const PxTransform& playerTransform, const PxF32 timestep)
{
//Increment the elapsed time
mTimeElapsed+=timestep;
//Work out the point on the crossing line of the next way-point that is closest to the player.
const PxTransform& nextWayPoint=mWayPoints[mProgress+1];
const PxVec3 v=nextWayPoint.p;
const PxVec3 w=nextWayPoint.q.getBasisVector0();
const PxVec3 p=playerTransform.p;
const PxVec3 pv=p-v;
const PxF32 t=pv.dot(w);
//Test if the player's position is inside the width of the line crossing the next way-point.
if(PxAbs(t) < LINEWIDTH)
{
//Now test if the shortest distance to the next crossing line is smaller than a threshold.
const PxVec3 linePos=v+w*t;
const PxVec3 diff=p-linePos;
const PxF32 dist2=diff.magnitudeSquared();
if(dist2<LINEDISTANCE2)
{
mProgress++;
}
}
if(mProgress == mNumWayPoints-1)
{
mMinTimeElapsed=PxMin(mTimeElapsed, mMinTimeElapsed);
mTimeElapsed=0;
mProgress=0;
}
}
float variableOscillator::updateVariableOscillator(float deltaTime)
{
float returnVal;
float halfRange;
mCumTime += deltaTime;
// has the function crossed a max or a min?
if ((mGoingUp && (mCumTime > (mPeriod / 2.0f))) ||
(!mGoingUp && (mCumTime > mPeriod)))
{
mStartVal = mLastVal;
if (mGoingUp)
{
mEndVal = computeEndVal(mStartVal, mMin);
}
else
{
mEndVal = computeEndVal(mStartVal, mMax);
mCumTime = mCumTime - mPeriod;
}
mGoingUp = !mGoingUp;
}
halfRange = 0.5f * PxAbs(mEndVal - mStartVal);
returnVal = -halfRange * PxCos(mCumTime * PxTwoPi / mPeriod) + halfRange + PxMin(mStartVal, mEndVal);
mLastVal = returnVal;
return(returnVal);
}
float ApexCudaProfileSession::flushProfileInfo(ProfileData& pd)
{
CUevent start = (CUevent)pd.start;
CUevent stop = (CUevent)pd.stop;
uint32_t op = 1;
float startTf = 0.f, stopTf = 0.f;
uint64_t startT = 0, stopT = 0;
CUT_SAFE_CALL(cuEventSynchronize(start));
CUT_SAFE_CALL(cuEventElapsedTime(&startTf, (CUevent)mTimer, start));
startT = static_cast<uint64_t>(startTf * mManager->mTimeFormat) ;
mMemBuf.write(&op, sizeof(op));
mMemBuf.write(&startT, sizeof(startT));
mMemBuf.write(&pd.id, sizeof(pd.id));
op = 2;
CUT_SAFE_CALL(cuEventSynchronize((CUevent)stop));
CUT_SAFE_CALL(cuEventElapsedTime(&stopTf, (CUevent)mTimer, (CUevent)stop));
stopT = static_cast<uint64_t>(stopTf * mManager->mTimeFormat);
mMemBuf.write(&op, sizeof(op));
mMemBuf.write(&stopT, sizeof(stopT));
mMemBuf.write(&pd.id, sizeof(pd.id));
CUT_SAFE_CALL(cuEventDestroy((CUevent)start));
CUT_SAFE_CALL(cuEventDestroy((CUevent)stop));
mFrameStart = PxMin(mFrameStart, startTf);
mFrameFinish = PxMax(mFrameFinish, stopTf);
return stopTf - startTf;
}
PxVec3 ComputeChassisAABBDimensions(const PxConvexMesh* chassisConvexMesh)
{
const PxU32 numChassisVerts=chassisConvexMesh->getNbVertices();
const PxVec3* chassisVerts=chassisConvexMesh->getVertices();
PxVec3 chassisMin(PX_MAX_F32,PX_MAX_F32,PX_MAX_F32);
PxVec3 chassisMax(-PX_MAX_F32,-PX_MAX_F32,-PX_MAX_F32);
for(PxU32 i=0;i<numChassisVerts;i++)
{
chassisMin.x=PxMin(chassisMin.x,chassisVerts[i].x);
chassisMin.y=PxMin(chassisMin.y,chassisVerts[i].y);
chassisMin.z=PxMin(chassisMin.z,chassisVerts[i].z);
chassisMax.x=PxMax(chassisMax.x,chassisVerts[i].x);
chassisMax.y=PxMax(chassisMax.y,chassisVerts[i].y);
chassisMax.z=PxMax(chassisMax.z,chassisVerts[i].z);
}
const PxVec3 chassisDims=chassisMax-chassisMin;
return chassisDims;
}
void VariableStepper::substepStrategy(const PxReal stepSize, PxU32& substepCount, PxReal& substepSize)
{
if(mAccumulator > mMaxSubStepSize)
mAccumulator = 0.0f;
// don't step less than the min step size, just accumulate
mAccumulator += stepSize;
if(mAccumulator < mMinSubStepSize)
{
substepCount = 0;
return;
}
substepCount = PxMin(PxU32(PxCeil(mAccumulator/mMaxSubStepSize)), mMaxSubSteps);
substepSize = PxMin(mAccumulator/substepCount, mMaxSubStepSize);
mAccumulator -= PxReal(substepCount)*substepSize;
}
PxConvexMesh* CreateWheelConvexMesh(const PxVec3* verts, const PxU32 numVerts)
{
//Extract the wheel radius and width from the aabb of the wheel convex mesh.
PxVec3 wheelMin(PX_MAX_F32,PX_MAX_F32,PX_MAX_F32);
PxVec3 wheelMax(-PX_MAX_F32,-PX_MAX_F32,-PX_MAX_F32);
for(PxU32 i=0;i<numVerts;i++)
{
wheelMin.x=PxMin(wheelMin.x,verts[i].x);
wheelMin.y=PxMin(wheelMin.y,verts[i].y);
wheelMin.z=PxMin(wheelMin.z,verts[i].z);
wheelMax.x=PxMax(wheelMax.x,verts[i].x);
wheelMax.y=PxMax(wheelMax.y,verts[i].y);
wheelMax.z=PxMax(wheelMax.z,verts[i].z);
}
const PxF32 wheelWidth=wheelMax.x-wheelMin.x;
const PxF32 wheelRadius=PxMax(wheelMax.y,wheelMax.z);
return CreateCylinderConvexMesh(wheelWidth,wheelRadius,8);
}
PxU32 NpArticulation::getLinks(PxArticulationLink** buffer, PxU32 bufferSize) const
{
NP_READ_CHECK(getOwnerScene());
const PxU32 size = mArticulationLinks.size();
const PxU32 writeCount = PxMin(size, bufferSize);
for(PxU32 i=0; i<writeCount; i++)
buffer[i] = mArticulationLinks[i];
return writeCount;
// return Ps::dumpPointerArray((const void**)mArticulationLinks.begin(), mArticulationLinks.size(), (void**)buffer, bufferSize);
}
void ComputeWheelWidthsAndRadii(PxConvexMesh** wheelConvexMeshes, PxF32* wheelWidths, PxF32* wheelRadii)
{
for(PxU32 i = 0; i < 4; i++)
{
const PxU32 numWheelVerts = wheelConvexMeshes[i]->getNbVertices();
const PxVec3* wheelVerts = wheelConvexMeshes[i]->getVertices();
PxVec3 wheelMin(PX_MAX_F32, PX_MAX_F32, PX_MAX_F32);
PxVec3 wheelMax(-PX_MAX_F32, -PX_MAX_F32, -PX_MAX_F32);
for(PxU32 j = 0; j < numWheelVerts; j++)
{
wheelMin.x = PxMin(wheelMin.x, wheelVerts[j].x);
wheelMin.y = PxMin(wheelMin.y, wheelVerts[j].y);
wheelMin.z = PxMin(wheelMin.z, wheelVerts[j].z);
wheelMax.x = PxMax(wheelMax.x, wheelVerts[j].x);
wheelMax.y = PxMax(wheelMax.y, wheelVerts[j].y);
wheelMax.z = PxMax(wheelMax.z, wheelVerts[j].z);
}
wheelWidths[i] = wheelMax.y - wheelMin.y;
wheelRadii[i] = PxMax(wheelMax.x, wheelMax.z) * 0.975f;
}
}
PX_FORCE_INLINE void RTreePage::computeBounds(RTreeNodeQ& newBounds)
{
RTreeValue _minx = MX, _miny = MX, _minz = MX, _maxx = MN, _maxy = MN, _maxz = MN;
for (PxU32 j = 0; j < RTreePage::SIZE; j++)
{
if (isEmpty(j))
continue;
_minx = PxMin(_minx, minx[j]);
_miny = PxMin(_miny, miny[j]);
_minz = PxMin(_minz, minz[j]);
_maxx = PxMax(_maxx, maxx[j]);
_maxy = PxMax(_maxy, maxy[j]);
_maxz = PxMax(_maxz, maxz[j]);
}
newBounds.minx = _minx;
newBounds.miny = _miny;
newBounds.minz = _minz;
newBounds.maxx = _maxx;
newBounds.maxy = _maxy;
newBounds.maxz = _maxz;
}
void FixedStepper::substepStrategy(const PxReal stepSize, PxU32& substepCount, PxReal& substepSize)
{
if(mAccumulator > mFixedSubStepSize)
mAccumulator = 0.0f;
// don't step less than the step size, just accumulate
mAccumulator += stepSize;
if(mAccumulator < mFixedSubStepSize)
{
substepCount = 0;
return;
}
substepSize = mFixedSubStepSize;
substepCount = PxMin(PxU32(mAccumulator/mFixedSubStepSize), mMaxSubSteps);
mAccumulator -= PxReal(substepCount)*substepSize;
}
//process value in range(0,1)
PX_FORCE_INLINE PxF32 processPositiveAnalogValue
(const PxF32 riseRate, const PxF32 fallRate,
const PxF32 currentVal, const PxF32 targetVal,
const PxF32 timestep)
{
PX_ASSERT(targetVal>=-0.01f && targetVal<=1.01f);
PxF32 val;
if(currentVal<targetVal)
{
val=currentVal + riseRate*timestep;
val=PxMin(val,targetVal);
}
else
{
val=currentVal - fallRate*timestep;
val=PxMax(val,targetVal);
}
return val;
}
void setMissingPropertiesToDefault( RepXNode* topNode, RepXReaderWriter& editor, const RepXDefaultEntry* defaults, PxU32 numDefaults, TNameOffsetMap& map )
{
for ( RepXNode* child = topNode->mFirstChild; child != NULL; child = child->mNextSibling )
setMissingPropertiesToDefault( child, editor, defaults, numDefaults, map );
const TNameOffsetMap::Entry* entry( map.find( topNode->mName ) );
if ( entry )
{
RepXReaderWriter& theReader( editor );
theReader.setNode( *topNode );
char nameBuffer[512] = {0};
size_t nameLen = strlen( topNode->mName );
//For each default property entry for this node type.
for ( const RepXDefaultEntry* item = defaults + entry->second; strncmp( item->name, topNode->mName, nameLen ) == 0; ++item )
{
bool childAdded = false;
const char* nameStart = item->name + nameLen;
++nameStart;
theReader.pushCurrentContext();
const char* str = nameStart;
while( *str )
{
const char *period = nextPeriod( str );
size_t len = PxMin( period - str, ptrdiff_t(1023) ); //can't be too careful these days.
memcpy( nameBuffer, str, len );
nameBuffer[len] = 0;
if ( theReader.gotoChild( nameBuffer ) == false )
{
childAdded = true;
theReader.addOrGotoChild( nameBuffer );
}
if (*period )
str = period + 1;
else
str = period;
}
if ( childAdded )
theReader.setCurrentItemValue( item->value );
theReader.popCurrentContext();
}
}
}
void CookingAbstract::PhysicalMesh::computeTriangleAreas()
{
smallestTriangleArea = largestTriangleArea = 0.0f;
if (indices == NULL || vertices == NULL)
{
return;
}
smallestTriangleArea = PX_MAX_F32;
for (PxU32 i = 0; i < numIndices; i += 3)
{
const PxVec3 edge1 = vertices[indices[i + 1]] - vertices[indices[i]];
const PxVec3 edge2 = vertices[indices[i + 2]] - vertices[indices[i]];
const PxF32 triangleArea = edge1.cross(edge2).magnitude();
largestTriangleArea = PxMax(largestTriangleArea, triangleArea);
smallestTriangleArea = PxMin(smallestTriangleArea, triangleArea);
}
}
PX_FORCE_INLINE PxU32 collideWithMeshTriangle(PxVec3& surfaceNormal, PxVec3& surfacePos,
PxVec3& proxSurfaceNormal, PxVec3& proxSurfacePos,
PxReal& ccTime, PxReal& distOldToSurface,
const PxVec3& oldPos, const PxVec3& newPos,
const PxVec3& origin, const PxVec3& e0,
const PxVec3& e1, bool hasCC,
const PxReal& collRadius, const PxReal& proxRadius)
{
PxU32 flags = 0;
PxReal collisionRadius2 = collRadius * collRadius;
PxReal proximityRadius2 = proxRadius * proxRadius;
PxVec3 motion = newPos - oldPos;
// dc and proximity tests
PxVec3 tmpV = origin - newPos;
PxReal a = e0.dot(e0);
PxReal b = e0.dot(e1);
PxReal c = e1.dot(e1);
PxReal d = e0.dot(tmpV);
PxReal e = e1.dot(tmpV);
PxVec3 coords;
coords.x = b*e - c*d; // s * det
coords.y = b*d - a*e; // t * det
coords.z = a*c - b*b; // det
bool insideCase = false;
PxVec3 clampedCoords(PxVec3(0));
if (coords.x <= 0.0f)
{
c = PxMax(c, FLT_MIN);
clampedCoords.y = -e/c;
}
else if (coords.y <= 0.0f)
{
a = PxMax(a, FLT_MIN);
clampedCoords.x = -d/a;
}
else if (coords.x + coords.y > coords.z)
{
PxReal denominator = a + c - b - b;
PxReal numerator = c + e - b - d;
denominator = PxMax(denominator, FLT_MIN);
clampedCoords.x = numerator / denominator;
clampedCoords.y = 1.0f - clampedCoords.x;
}
else // all inside
{
PxReal tmpF = PxMax(coords.z, FLT_MIN);
tmpF = 1.0f / tmpF;
clampedCoords.x = coords.x * tmpF;
clampedCoords.y = coords.y * tmpF;
insideCase = true;
}
clampedCoords.x = PxMax(clampedCoords.x, 0.0f);
clampedCoords.y = PxMax(clampedCoords.y, 0.0f);
clampedCoords.x = PxMin(clampedCoords.x, 1.0f);
clampedCoords.y = PxMin(clampedCoords.y, 1.0f);
// Closest point to particle inside triangle
PxVec3 closest = origin + e0 * clampedCoords.x + e1 * clampedCoords.y;
PxVec3 triangleOffset = newPos - closest;
PxReal triangleDistance2 = triangleOffset.magnitudeSquared();
PxVec3 triangleNormal = e0.cross(e1);
PxReal e0e1Span = triangleNormal.magnitude();
bool isInFront = triangleOffset.dot(triangleNormal) > 0.0f;
// MS: Possible optimzation
/*
if (isInFront && (triangleDistance2 >= proximityRadius2))
return flags;
*/
bool isInProximity = insideCase && (triangleDistance2 < proximityRadius2) && isInFront;
bool isInDiscrete = (triangleDistance2 < collisionRadius2) && isInFront;
if (!hasCC)
{
// Only apply discrete and proximity collisions if no continuous collisions was detected so far (for any colliding shape)
if (isInDiscrete)
{
if (triangleDistance2 > PXS_FLUID_COLL_TRI_DISTANCE)
{
surfaceNormal = triangleOffset * PxRecipSqrt(triangleDistance2);
}
else
{
surfaceNormal = triangleNormal * (1.0f / e0e1Span);
}
surfacePos = closest + (surfaceNormal * collRadius);
flags |= PXS_FLUID_COLL_FLAG_L_DC;
}
if (isInProximity)
{
proxSurfaceNormal = triangleNormal * (1.0f / e0e1Span);
proxSurfacePos = closest + (proxSurfaceNormal * collRadius);
flags |= PXS_FLUID_COLL_FLAG_L_PROX;
tmpV = (oldPos - origin); //this time it's not the newPosition offset.
distOldToSurface = proxSurfaceNormal.dot(tmpV); // Need to return the distance to decide which constraints should be thrown away
}
}
if (!isInDiscrete && !isInProximity)
{
// cc test (let's try only executing this if no discrete coll, or proximity happend).
tmpV = origin - oldPos; //this time it's not the newPosition offset.
PxReal pDistN = triangleNormal.dot(tmpV);
PxReal rLengthN = triangleNormal.dot(motion);
if (pDistN > 0.0f || rLengthN >= pDistN)
return flags;
//we are in the half closed interval [0.0f, 1.0)
PxReal t = pDistN / rLengthN;
PX_ASSERT((t >= 0.0f) && (t < 1.0f));
PxVec3 relativePOSITION = (motion * t);
PxVec3 testPoint = oldPos + relativePOSITION;
// a,b,c and coords.z don't depend on test point -> still valid
tmpV = origin - testPoint;
d = e0.dot(tmpV);
e = e1.dot(tmpV);
coords.x = b*e - c*d;
coords.y = b*d - a*e;
//maybe we don't need this for rare case leaking on triangle boundaries?
PxReal eps = coords.z * PXS_FLUID_COLL_RAY_EPSILON_FACTOR;
if ((coords.x >= -eps) && (coords.y >= -eps) && (coords.x + coords.y <= coords.z + eps))
{
PxReal invLengthN = (1.0f / e0e1Span);
distOldToSurface = -pDistN * invLengthN; // Need to return the distance to decide which constraints should be thrown away
surfaceNormal = triangleNormal * invLengthN;
//surfacePos = testPoint + (surfaceNormal * collRadius);
computeContinuousTargetPosition(surfacePos, oldPos, relativePOSITION, surfaceNormal, collRadius);
ccTime = t;
flags |= PXS_FLUID_COLL_FLAG_L_CC;
}
}
return flags;
}
void PxVehicleCopyDynamicsData(const PxVehicleCopyDynamicsMap& wheelMap, const PxVehicleWheels& src, PxVehicleWheels* trg)
{
PX_CHECK_AND_RETURN(trg, "PxVehicleCopyDynamicsData requires that trg is a valid vehicle pointer");
PX_CHECK_AND_RETURN(src.getVehicleType() == trg->getVehicleType(), "PxVehicleCopyDynamicsData requires that both src and trg are the same type of vehicle");
#ifdef PX_CHECKED
{
const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();
PxU8 copiedWheelsSrc[PX_MAX_NB_WHEELS];
PxMemZero(copiedWheelsSrc, sizeof(PxU8) * PX_MAX_NB_WHEELS);
PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);
for(PxU32 i = 0; i < PxMin(numWheelsSrc, numWheelsTrg); i++)
{
const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
PX_CHECK_AND_RETURN(srcWheelId < numWheelsSrc, "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
PX_CHECK_AND_RETURN(0 == copiedWheelsSrc[srcWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
copiedWheelsSrc[srcWheelId] = 1;
const PxU32 trgWheelId = wheelMap.targetWheelIds[i];
PX_CHECK_AND_RETURN(trgWheelId < numWheelsTrg, "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
PX_CHECK_AND_RETURN(0 == setWheelsTrg[trgWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
setWheelsTrg[trgWheelId]=1;
}
}
#endif
const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();
//Set all dynamics data on the target to zero.
//Be aware that setToRestState sets the rigid body to
//rest so set the momentum back after calling setToRestState.
PxRigidDynamic* actorTrg = trg->getRigidDynamicActor();
PxVec3 linVel = actorTrg->getLinearVelocity();
PxVec3 angVel = actorTrg->getAngularVelocity();
switch(src.getVehicleType())
{
case PxVehicleTypes::eDRIVE4W:
((PxVehicleDrive4W*)trg)->setToRestState();
break;
case PxVehicleTypes::eDRIVENW:
((PxVehicleDriveNW*)trg)->setToRestState();
break;
case PxVehicleTypes::eDRIVETANK:
((PxVehicleDriveTank*)trg)->setToRestState();
break;
case PxVehicleTypes::eNODRIVE:
((PxVehicleNoDrive*)trg)->setToRestState();
break;
default:
break;
}
actorTrg->setLinearVelocity(linVel);
actorTrg->setAngularVelocity(angVel);
//Keep a track of the wheels on trg that have their dynamics data set as a copy from src.
PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);
//Keep a track of the average wheel rotation speed of all enabled wheels on src.
PxU32 numEnabledWheelsSrc = 0;
PxF32 accumulatedWheelRotationSpeedSrc = 0.0f;
//Copy wheel dynamics data from src wheels to trg wheels.
//Track the target wheels that have been given dynamics data from src wheels.
//Compute the accumulated wheel rotation speed of all enabled src wheels.
const PxU32 numMappedWheels = PxMin(numWheelsSrc, numWheelsTrg);
for(PxU32 i = 0; i < numMappedWheels; i++)
{
const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
const PxU32 trgWheelId = wheelMap.targetWheelIds[i];
trg->mWheelsDynData.copy(src.mWheelsDynData, srcWheelId, trgWheelId);
setWheelsTrg[trgWheelId] = 1;
if(!src.mWheelsSimData.getIsWheelDisabled(srcWheelId))
{
numEnabledWheelsSrc++;
accumulatedWheelRotationSpeedSrc += src.mWheelsDynData.getWheelRotationSpeed(srcWheelId);
}
}
//Compute the average wheel rotation speed of src.
PxF32 averageWheelRotationSpeedSrc = 0;
if(numEnabledWheelsSrc > 0)
{
averageWheelRotationSpeedSrc = (accumulatedWheelRotationSpeedSrc/ (1.0f * numEnabledWheelsSrc));
}
//For wheels on trg that have not had their dynamics data copied from src just set
//their wheel rotation speed to the average wheel rotation speed.
for(PxU32 i = 0; i < numWheelsTrg; i++)
{
if(0 == setWheelsTrg[i] && !trg->mWheelsSimData.getIsWheelDisabled(i))
{
trg->mWheelsDynData.setWheelRotationSpeed(i, averageWheelRotationSpeedSrc);
}
}
//Copy the engine rotation speed/gear states/autobox states/etc.
switch(src.getVehicleType())
{
case PxVehicleTypes::eDRIVE4W:
case PxVehicleTypes::eDRIVENW:
case PxVehicleTypes::eDRIVETANK:
{
const PxVehicleDriveDynData& driveDynDataSrc = ((const PxVehicleDrive&)src).mDriveDynData;
PxVehicleDriveDynData* driveDynDataTrg = &((PxVehicleDrive*)trg)->mDriveDynData;
*driveDynDataTrg = driveDynDataSrc;
}
break;
default:
break;
}
}
PX_FORCE_INLINE void RTreeNodeQ::grow(const RTreeNodeQ& node)
{
minx = PxMin(minx, node.minx); miny = PxMin(miny, node.miny); minz = PxMin(minz, node.minz);
maxx = PxMax(maxx, node.maxx); maxy = PxMax(maxy, node.maxy); maxz = PxMax(maxz, node.maxz);
}
void RTree::refitAllStaticTree(CallbackRefit& cb, PxBounds3* retBounds)
{
PxU8* treeNodes8 = PX_IS_X64 ? CAST_U8(get64BitBasePage()) : CAST_U8((mFlags & IS_DYNAMIC) ? NULL : mPages);
// since pages are ordered we can scan back to front and the hierarchy will be updated
for (PxI32 iPage = PxI32(mTotalPages)-1; iPage>=0; iPage--)
{
RTreePage& page = mPages[iPage];
for (PxU32 j = 0; j < RTREE_PAGE_SIZE; j++)
{
if (page.isEmpty(j))
continue;
if (page.isLeaf(j))
{
Vec3V childMn, childMx;
cb.recomputeBounds(page.ptrs[j]-1, childMn, childMx); // compute the bound around triangles
PxVec3 mn3, mx3;
V3StoreU(childMn, mn3);
V3StoreU(childMx, mx3);
page.minx[j] = mn3.x; page.miny[j] = mn3.y; page.minz[j] = mn3.z;
page.maxx[j] = mx3.x; page.maxy[j] = mx3.y; page.maxz[j] = mx3.z;
} else
{
const RTreePage* child = (const RTreePage*)(treeNodes8 + page.ptrs[j]);
PX_COMPILE_TIME_ASSERT(RTREE_PAGE_SIZE == 4);
bool first = true;
for (PxU32 k = 0; k < RTREE_PAGE_SIZE; k++)
{
if (child->isEmpty(k))
continue;
if (first)
{
page.minx[j] = child->minx[k]; page.miny[j] = child->miny[k]; page.minz[j] = child->minz[k];
page.maxx[j] = child->maxx[k]; page.maxy[j] = child->maxy[k]; page.maxz[j] = child->maxz[k];
first = false;
} else
{
page.minx[j] = PxMin(page.minx[j], child->minx[k]);
page.miny[j] = PxMin(page.miny[j], child->miny[k]);
page.minz[j] = PxMin(page.minz[j], child->minz[k]);
page.maxx[j] = PxMax(page.maxx[j], child->maxx[k]);
page.maxy[j] = PxMax(page.maxy[j], child->maxy[k]);
page.maxz[j] = PxMax(page.maxz[j], child->maxz[k]);
}
}
}
}
}
if (retBounds)
{
RTreeNodeQ bound1;
for (PxU32 ii = 0; ii<mNumRootPages; ii++)
{
mPages[ii].computeBounds(bound1);
if (ii == 0)
{
retBounds->minimum = PxVec3(bound1.minx, bound1.miny, bound1.minz);
retBounds->maximum = PxVec3(bound1.maxx, bound1.maxy, bound1.maxz);
} else
{
retBounds->minimum = retBounds->minimum.minimum(PxVec3(bound1.minx, bound1.miny, bound1.minz));
retBounds->maximum = retBounds->maximum.maximum(PxVec3(bound1.maxx, bound1.maxy, bound1.maxz));
}
}
}
#ifdef PX_CHECKED
validate(&cb);
#endif
}
void dampVec3(const PxVec3& oldPosition, PxVec3& newPosition, PxF32 timestep)
{
PxF32 t = 0.7f * timestep * 8.0f;
t = PxMin(t, 1.0f);
newPosition = oldPosition * (1 - t) + newPosition * t;
}
void PxsFluidDynamics::updatePacketLocalHash(PxsSphUpdateType updateType, PxVec3* forceBuf, PxsFluidParticle* particles, const PxsParticleCell& packet,
const PxsFluidPacketSections& packetSections, const PxsFluidPacketHaloRegions& haloRegions,
PxsFluidDynamicsTempBuffers& tempBuffers)
{
// Particle index lists for local hash of particle cells (for two subpackets A and B).
PxU32* particleIndicesSpA = tempBuffers.indicesSubpacketA;
PxU32* particleIndicesSpB = tempBuffers.indicesSubpacketB;
// Local hash tables for particle cells (for two subpackets A and B).
PxsParticleCell* particleCellsSpA = tempBuffers.cellHashTableSubpacketA;
PxsParticleCell* particleCellsSpB = tempBuffers.cellHashTableSubpacketB;
PxVec3 packetCorner = PxVec3(PxReal(packet.coords.x), PxReal(packet.coords.y), PxReal(packet.coords.z)) * mParams.packetSize;
PxU32 particlesLeftA0 = packet.numParticles;
PxsFluidParticle* particlesSpA0 = particles + packet.firstParticle;
PxVec3* forceBufA0 = forceBuf + packet.firstParticle;
while (particlesLeftA0)
{
PxU32 numParticlesSpA = PxMin(particlesLeftA0, static_cast<PxU32>(PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY));
// Make sure the number of hash buckets is a power of 2 (requirement for the used hash function)
const PxU32 numCellHashBucketsSpA = Ps::nextPowerOfTwo(numParticlesSpA + 1);
PX_ASSERT(numCellHashBucketsSpA <= tempBuffers.cellHashMaxSize);
// Get local cell hash for the current subpacket
PxsFluidSpatialHash::buildLocalHash(particlesSpA0, numParticlesSpA, particleCellsSpA,
particleIndicesSpA, tempBuffers.hashKeys, numCellHashBucketsSpA, mParams.cellSizeInv, packetCorner);
//---------------------------------------------------------------------------------------------------
//
// Compute particle interactions between particles within the current subpacket.
//
updateCellsSubpacket(updateType, forceBufA0, particlesSpA0, particleCellsSpA, particleIndicesSpA, numCellHashBucketsSpA, mParams, tempBuffers);
//---------------------------------------------------------------------------------------------------
//
// Compute particle interactions between particles of current subpacket and particles
// of other subpackets within the same packet (i.e., we process all subpacket pairs).
//
PxU32 particlesLeftB = particlesLeftA0 - numParticlesSpA;
PxsFluidParticle* particlesSpB = particlesSpA0 + numParticlesSpA;
PxVec3* forceBufB = forceBufA0 + numParticlesSpA;
while (particlesLeftB)
{
PxU32 numParticlesSpB = PxMin(particlesLeftB, static_cast<PxU32>(PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY));
// Make sure the number of hash buckets is a power of 2 (requirement for the used hash function)
const PxU32 numCellHashBucketsSpB = Ps::nextPowerOfTwo(numParticlesSpB + 1);
PX_ASSERT(numCellHashBucketsSpB <= tempBuffers.cellHashMaxSize);
// Get local cell hash for other subpacket
PxsFluidSpatialHash::buildLocalHash(particlesSpB, numParticlesSpB, particleCellsSpB, particleIndicesSpB, tempBuffers.hashKeys,
numCellHashBucketsSpB, mParams.cellSizeInv, packetCorner);
// For the cells of subpacket A, find neighboring cells in the subpacket B and compute particle interactions.
updateCellsSubpacketPair(updateType, forceBufA0, forceBufB, particlesSpA0, particlesSpB, particleCellsSpA, particleCellsSpB,
particleIndicesSpA, particleIndicesSpB, numCellHashBucketsSpA, numCellHashBucketsSpB, true, mParams, tempBuffers, numParticlesSpA < numParticlesSpB);
particlesLeftB -= numParticlesSpB;
particlesSpB += numParticlesSpB;
forceBufB += numParticlesSpB;
}
particlesLeftA0 -= numParticlesSpA;
particlesSpA0 += numParticlesSpA;
forceBufA0 += numParticlesSpA;
}
//---------------------------------------------------------------------------------------------------
//
// Compute particle interactions between particles of sections of the current packet and particles of neighboring
// halo regions
//
PX_ASSERT(PXS_FLUID_BRUTE_FORCE_PARTICLE_THRESHOLD_HALO_VS_SECTION <= PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY);
if (haloRegions.maxNumParticles != 0)
{
for(PxU32 s=0; s < 26; s++)
{
PxU32 numSectionParticles = packetSections.numParticles[s];
if (numSectionParticles == 0)
continue;
bool sectionEnablesBruteForce = (numSectionParticles <= PXS_FLUID_BRUTE_FORCE_PARTICLE_THRESHOLD_HALO_VS_SECTION);
SectionToHaloTable& neighborHaloRegions = sSectionToHaloTable[s];
PxU32 numHaloNeighbors = neighborHaloRegions.numHaloRegions;
PxU32 particlesLeftA = numSectionParticles;
PxsFluidParticle* particlesSpA = particles + packetSections.firstParticle[s];
PxVec3* forceBufA = forceBuf + packetSections.firstParticle[s];
while (particlesLeftA)
{
PxU32 numParticlesSpA = PxMin(particlesLeftA, static_cast<PxU32>(PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY));
// Compute particle interactions between particles of the current subpacket (of the section)
// and particles of neighboring halo regions relevant.
//Process halo regions which need local hash building first.
bool isLocalHashValid = false;
// Make sure the number of hash buckets is a power of 2 (requirement for the used hash function)
const PxU32 numCellHashBucketsSpA = Ps::nextPowerOfTwo(numParticlesSpA + 1);
PX_ASSERT(numCellHashBucketsSpA <= tempBuffers.cellHashMaxSize);
#if MERGE_HALO_REGIONS
//Read halo region particles into temporary buffer
PxU32 numMergedHaloParticles = 0;
for(PxU32 h = 0; h < numHaloNeighbors; h++)
{
PxU32 haloRegionIdx = neighborHaloRegions.haloRegionIndices[h];
PxU32 numHaloParticles = haloRegions.numParticles[haloRegionIdx];
// chunk regions into subpackets!
PxU32 particlesLeftB = numHaloParticles;
PxsFluidParticle* particlesSpB = particles + haloRegions.firstParticle[haloRegionIdx];
PxVec3* forceBufB = forceBuf + haloRegions.firstParticle[haloRegionIdx];
while (particlesLeftB)
{
PxU32 numParticlesSpB = PxMin(particlesLeftB, static_cast<PxU32>(PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY));
// if there are plenty of particles already, don't bother to do the copy for merging.
if (numParticlesSpB > PXS_FLUID_BRUTE_FORCE_PARTICLE_THRESHOLD_HALO_VS_SECTION)
{
updateSubpacketPairHalo(forceBufA, particlesSpA, numParticlesSpA, particleCellsSpA, particleIndicesSpA, isLocalHashValid, numCellHashBucketsSpA,
forceBufB, particlesSpB, numParticlesSpB, particleCellsSpB, particleIndicesSpB, packetCorner, updateType, hashKeyArray, tempBuffers);
}
else
{
if (numMergedHaloParticles + numParticlesSpB > PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY)
{
//flush
updateSubpacketPairHalo(forceBufA, particlesSpA, numParticlesSpA, particleCellsSpA, particleIndicesSpA, isLocalHashValid, numCellHashBucketsSpA,
tempBuffers.mergedHaloRegions, numMergedHaloParticles, particleCellsSpB, particleIndicesSpB, packetCorner, updateType, hashKeyArray, tempBuffers);
numMergedHaloParticles = 0;
}
for (PxU32 k = 0; k < numParticlesSpB; ++k)
tempBuffers.mergedHaloRegions[numMergedHaloParticles++] = particlesSpB[k];
}
particlesLeftB -= numParticlesSpB;
particlesSpB += numParticlesSpB;
}
}
//flush
updateSubpacketPairHalo(forceBufA, particlesSpA, numParticlesSpA, particleCellsSpA, particleIndicesSpA, isLocalHashValid, numCellHashBucketsSpA,
tempBuffers.mergedHaloRegions, numMergedHaloParticles, particleCellsSpB, particleIndicesSpB, packetCorner, updateType, hashKeyArray, tempBuffers);
#else // MERGE_HALO_REGIONS
for(PxU32 h = 0; h < numHaloNeighbors; h++)
{
PxU32 haloRegionIdx = neighborHaloRegions.haloRegionIndices[h];
PxU32 numHaloParticles = haloRegions.numParticles[haloRegionIdx];
bool haloRegionEnablesBruteForce = (numHaloParticles <= PXS_FLUID_BRUTE_FORCE_PARTICLE_THRESHOLD_HALO_VS_SECTION);
if (sectionEnablesBruteForce && haloRegionEnablesBruteForce)
continue;
if (!isLocalHashValid)
{
// Get local cell hash for the current subpacket
PxsFluidSpatialHash::buildLocalHash(particlesSpA, numParticlesSpA, particleCellsSpA,
particleIndicesSpA, tempBuffers.hashKeys, numCellHashBucketsSpA, mParams.cellSizeInv, packetCorner);
isLocalHashValid = true;
}
PxU32 particlesLeftB = numHaloParticles;
PxsFluidParticle* particlesSpB = particles + haloRegions.firstParticle[haloRegionIdx];
while (particlesLeftB)
{
PxU32 numParticlesSpB = PxMin(particlesLeftB, static_cast<PxU32>(PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY));
// It is important that no data is written to particles in halo regions since they belong to
// a neighboring packet. The interaction effect of the current packet on the neighboring packet will be
// considered when the neighboring packet is processed.
// Make sure the number of hash buckets is a power of 2 (requirement for the used hash function)
const PxU32 numCellHashBucketsSpB = Ps::nextPowerOfTwo(numParticlesSpB + 1);
PX_ASSERT(numCellHashBucketsSpB <= tempBuffers.cellHashMaxSize);
// Get local cell hash for other subpacket
PxsFluidSpatialHash::buildLocalHash(particlesSpB, numParticlesSpB, particleCellsSpB, particleIndicesSpB, tempBuffers.hashKeys,
numCellHashBucketsSpB, mParams.cellSizeInv, packetCorner);
// For the cells of subpacket A, find neighboring cells in the subpacket B and compute particle interactions.
updateCellsSubpacketPair(updateType, forceBufA, NULL, particlesSpA, particlesSpB, particleCellsSpA, particleCellsSpB,
particleIndicesSpA, particleIndicesSpB, numCellHashBucketsSpA, numCellHashBucketsSpB, false, mParams, tempBuffers, numParticlesSpA > numParticlesSpB);
particlesLeftB -= numParticlesSpB;
particlesSpB += numParticlesSpB;
}
}
//Now process halo regions which don't need local hash building.
PxU32 mergedIndexCount = 0;
for(PxU32 h = 0; h < numHaloNeighbors; h++)
{
PxU32 haloRegionIdx = neighborHaloRegions.haloRegionIndices[h];
PxU32 numHaloParticles = haloRegions.numParticles[haloRegionIdx];
if (numHaloParticles == 0)
continue;
bool haloRegionEnablesBruteForce = (numHaloParticles <= PXS_FLUID_BRUTE_FORCE_PARTICLE_THRESHOLD_HALO_VS_SECTION);
if (!sectionEnablesBruteForce || !haloRegionEnablesBruteForce)
continue;
// The section and the halo region do not have enough particles to make it worth
// building a local cell hash --> use brute force approach
// This is given by the brute force condition (haloRegionEnablesBruteForce). Its necessary to
// make sure a halo region alone fits into the merge buffer.
PX_ASSERT(numHaloParticles <= PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY);
if (mergedIndexCount + numHaloParticles > PXS_FLUID_SUBPACKET_PARTICLE_LIMIT_FORCE_DENSITY)
{
updateParticleGroupPair(forceBufA, NULL, particlesSpA, particles,
tempBuffers.orderedIndicesSubpacket, numSectionParticles,
tempBuffers.mergedIndices, mergedIndexCount,
false, updateType == PXS_SPH_DENSITY,
mParams, tempBuffers.simdPositionsSubpacket, tempBuffers.indexStream);
mergedIndexCount = 0;
}
PxU32 hpIndex = haloRegions.firstParticle[haloRegionIdx];
for (PxU32 k = 0; k < numHaloParticles; k++)
tempBuffers.mergedIndices[mergedIndexCount++] = hpIndex++;
}
if (mergedIndexCount > 0)
{
updateParticleGroupPair(forceBufA, NULL, particlesSpA, particles,
tempBuffers.orderedIndicesSubpacket, numSectionParticles,
tempBuffers.mergedIndices, mergedIndexCount,
false, updateType == PXS_SPH_DENSITY,
mParams, tempBuffers.simdPositionsSubpacket, tempBuffers.indexStream);
}
#endif // MERGE_HALO_REGIONS
particlesLeftA -= numParticlesSpA;
particlesSpA += numParticlesSpA;
forceBufA += numParticlesSpA;
}
}
}
}
void ParticleEmitterPressure::stepInternal(ParticleData& particles, PxReal dt, const PxVec3& externalAcceleration, PxReal maxParticleVelocity)
{
PX_ASSERT(mNumX > 0 && mNumY > 0);
mSimulationAcceleration = externalAcceleration;
mSimulationMaxVelocity = maxParticleVelocity;
PxU32 numEmittedParticles = 0;
PxU32 maxParticlesPerStep = (PxU32)physx::shdfnd::floor(mMaxRate*dt);
PxU32 maxParticles = PxMin(particles.maxParticles - particles.numParticles, maxParticlesPerStep);
PxU32 siteNr = 0;
for(PxU32 y = 0; y != mNumY; y++)
{
PxReal offset = 0.0f;
if (y%2) offset = mSpacingX * 0.5f;
for(PxU32 x = 0; x != mNumX; x++)
{
if (isOutsideShape(x,y,offset))
continue;
SiteData& siteData = mSites[siteNr];
//position noise
PxVec3 posNoise;
posNoise.x = randInRange(-mRandomPos.x, mRandomPos.x);
posNoise.y = randInRange(-mRandomPos.y, mRandomPos.y);
//special code for Z noise
if (!siteData.predecessor)
siteData.noiseZ = randInRange(-mRandomPos.z, mRandomPos.z);
else
{
PxReal noiseZOffset = PxMin(mMaxZNoiseOffset, mRandomPos.z);
siteData.noiseZ += randInRange(-noiseZOffset, noiseZOffset);
siteData.noiseZ = PxClamp(siteData.noiseZ, mRandomPos.z, -mRandomPos.z);
}
posNoise.z = siteData.noiseZ;
//set position
PxVec3 sitePos = mBasePos + mAxisX*(offset+mSpacingX*x) + mAxisY*(mSpacingY*y) + mAxisZ*siteData.noiseZ;
PxVec3 particlePos = sitePos + mAxisX*posNoise.x + mAxisY*posNoise.y;
PxVec3 siteVel;
computeSiteVelocity(siteVel, particlePos);
if (siteData.predecessor)
{
predictPredecessorPos(siteData, dt);
}
else {
bool isSpawned = spawnParticle(particles, numEmittedParticles, maxParticles, particlePos, siteVel);
if(isSpawned)
{
updatePredecessor(siteData, particlePos, siteVel);
}
else
{
siteData.predecessor = false;
return;
}
}
bool allSpawned = stepEmissionSite(siteData, particles, numEmittedParticles, maxParticles, sitePos, siteVel, dt);
if(!allSpawned)
return;
siteNr++;
}
}
}
