uint32_t hsQueueStream::Read(uint32_t byteCount, void * buffer)
{
hsAssert(fWriteCursor >= 0 && fWriteCursor < fSize,"hsQueueStream: WriteCursor out of range.");
hsAssert(fReadCursor >= 0 && fReadCursor < fSize,"hsQueueStream: ReadCursor out of range.");
int32_t limit, length, total;
limit = fWriteCursor >= fReadCursor ? fWriteCursor : fSize;
length = hsMinimum(limit-fReadCursor,byteCount);
HSMemory::BlockMove(fQueue+fReadCursor,buffer,length);
fReadCursor += length;
fReadCursor %= fSize;
total = length;
if (length < byteCount && limit != fWriteCursor)
{
limit = fWriteCursor;
length = hsMinimum(limit,byteCount-length);
HSMemory::BlockMove(fQueue,static_cast<char*>(buffer)+total,length);
fReadCursor = length;
total += length;
}
return total;
}
uint32_t hsQueueStream::Write(uint32_t byteCount, const void* buffer)
{
hsAssert(fWriteCursor >= 0 && fWriteCursor < fSize,"hsQueueStream: WriteCursor out of range.");
hsAssert(fReadCursor >= 0 && fReadCursor < fSize,"hsQueueStream: ReadCursor out of range.");
int32_t length;
length = hsMinimum(fSize-fWriteCursor,byteCount);
HSMemory::BlockMove(buffer,fQueue+fWriteCursor,length);
if (fReadCursor > fWriteCursor)
{
#if 0
if (fReadCursor < fWriteCursor+length+1)
hsStatusMessage("ReadCursor wrapped\n");
#endif
fReadCursor = hsMaximum(fReadCursor,fWriteCursor+length+1);
fReadCursor %= fSize;
}
fWriteCursor += length;
fWriteCursor %= fSize;
if (length < byteCount)
{
Write(byteCount - length,static_cast<const char*>(buffer)+length);
}
return byteCount;
}
void plDynamicWalkingStrategy::Apply(float delSecs)
{
hsVector3 velocity = fController->GetLinearVelocity();
hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();
// Add in gravity if the avatar's z velocity isn't being set explicitly
if (hsABS(velocity.fZ) < 0.001f)
{
// Get our previous z velocity. If we're on the ground, clamp it to zero at
// the largest, so we won't launch into the air if we're running uphill.
float prevZVel = achievedVelocity.fZ;
if (IsOnGround())
prevZVel = hsMinimum(prevZVel, 0.f);
velocity.fZ = prevZVel + (kGravity * delSecs);
}
if (velocity.fZ < kTerminalVelocity)
velocity.fZ = kTerminalVelocity;
fController->SetPushingPhysical(nil);
fController->SetFacingPushingPhysical(false);
fGroundHit = fFalseGround = false;
float groundZVelocity;
if (ICheckForGround(groundZVelocity))
velocity.fZ += groundZVelocity;
fController->SetLinearVelocitySim(velocity);
}
void hsQueueStream::Skip(uint32_t deltaByteCount)
{
int32_t limit, length;
limit = fWriteCursor >= fReadCursor ? fWriteCursor : fSize;
length = hsMinimum(limit-fReadCursor,deltaByteCount);
fReadCursor += length;
if (length < deltaByteCount && limit != fWriteCursor)
{
limit = fWriteCursor;
length = hsMinimum(limit,deltaByteCount-length);
fReadCursor = length;
}
else
{
fReadCursor %= fSize;
}
}
static void MakeBoxFromHull(NxConvexMesh* convexMesh, NxBoxShapeDesc& box)
{
NxConvexMeshDesc desc;
convexMesh->saveToDesc(desc);
float minX, minY, minZ, maxX, maxY, maxZ;
minX = minY = minZ = FLT_MAX;
maxX = maxY = maxZ = -FLT_MAX;
for (int i = 0; i < desc.numVertices; i++)
{
float* point = (float*)(((char*)desc.points) + desc.pointStrideBytes*i);
float x = point[0];
float y = point[1];
float z = point[2];
minX = hsMinimum(minX, x);
minY = hsMinimum(minY, y);
minZ = hsMinimum(minZ, z);
maxX = hsMaximum(maxX, x);
maxY = hsMaximum(maxY, y);
maxZ = hsMaximum(maxZ, z);
}
float xWidth = maxX - minX;
float yWidth = maxY - minY;
float zWidth = maxZ - minZ;
box.dimensions.x = xWidth / 2;
box.dimensions.y = yWidth / 2;
box.dimensions.z = zWidth / 2;
//hsMatrix44 mat;
//box.localPose.getRowMajor44(&mat.fMap[0][0]);
hsPoint3 trans(minX + (xWidth / 2), minY + (yWidth / 2), minZ + (zWidth / 2));
//mat.SetTranslate(&trans);
//box.localPose.setRowMajor44(&mat.fMap[0][0]);
hsMatrix44 boxL2W;
boxL2W.Reset();
boxL2W.SetTranslate(&trans);
plPXConvert::Matrix(boxL2W, box.localPose);
}
hsBool plViewTransform::Union(const plViewTransform& view)
{
hsPoint3 mins;
hsPoint3 maxs;
int i;
for( i = 0; i < 3; i++ )
{
mins[i] = hsMinimum(fMin[i], view.fMin[i]);
maxs[i] = hsMaximum(fMax[i], view.fMax[i]);
}
SetView(mins, maxs);
return true;
}
hsBool plViewTransform::Intersect(const plViewTransform& view)
{
hsPoint3 mins;
hsPoint3 maxs;
hsBool retVal = true;
int i;
for( i = 0; i < 3; i++ )
{
mins[i] = hsMaximum(fMin[i], view.fMin[i]);
maxs[i] = hsMinimum(fMax[i], view.fMax[i]);
if( mins[i] >= maxs[i] )
{
mins[i] = maxs[i] = (mins[i] + maxs[i]) * 0.5f;
retVal = false;
}
}
SetView(mins, maxs);
return retVal;
}
void plDispatchLog::DumpMsg(plMessage* msg, int numReceivers, int sendTimeMs, int32_t indent)
{
if (!msg)
return;
bool found=fIncludeTypes.IsBitSet(msg->ClassIndex());
if (found && !hsCheckBits(fFlags, plDispatchLogBase::kInclude))
// it's an exclude list and we found it
return;
if (!found && hsCheckBits(fFlags, plDispatchLogBase::kInclude))
// it's an include list and we didn't find it
return;
static float lastTime=0;
float curTime = (float)hsTimer::GetSysSeconds();
if (lastTime!=curTime)
{
// add linebreak for new frame
fLog->AddLine("\n");
}
float sendTime = hsTimer::FullTicksToMs(hsTimer::GetFullTickCount() - fStartTicks);
char indentStr[50];
indent = hsMinimum(indent, sizeof(indentStr)-1);
memset(indentStr, ' ', indent);
indentStr[indent] = '\0';
fLog->AddLineF("%sDispatched (%d) %d ms: time=%d CName=%s, sndr=%s, rcvr(%d)=%s, flags=0x%lx, tstamp=%f\n",
indentStr, numReceivers, sendTimeMs,
int(sendTime), msg->ClassName(), msg->fSender?msg->fSender->GetName().c_str():"nil",
msg->GetNumReceivers(), msg->GetNumReceivers() && msg->GetReceiver(0)
? msg->GetReceiver(0)->GetName().c_str():"nil",
msg->fBCastFlags, msg->fTimeStamp);
lastTime=curTime;
}
///////////////////////////
//Walking Strategy
void plWalkingStrategy::Apply(float delSecs)
{
//Apply Should Only be Called from a PhysicalControllerCore
hsAssert(fCore,"No Core shouldn't be Applying");
uint32_t collideFlags =
1<<plSimDefs::kGroupStatic |
1<<plSimDefs::kGroupAvatarBlocker |
1<<plSimDefs::kGroupDynamic;
if(!fCore->IsSeeking())
{
collideFlags|=(1<<plSimDefs::kGroupExcludeRegion);
}
bool OnTopOfAnimatedPhys=false;
hsVector3 LinearVelocity=fCore->GetLinearVelocity();
hsVector3 AchievedLinearVelocity=fCore->GetAchievedLinearVelocity();
hsPoint3 positionBegin;
fCore->GetPositionSim(positionBegin);
bool recovered=false;
if (fCore->IsKinematic())
{
plSceneObject* so = plSceneObject::ConvertNoRef(fOwner->ObjectIsLoaded());
if (so)
{
// If we've been moved since the last physics update (somebody warped us),
// update the physics before we apply velocity.
const hsMatrix44& l2w = so->GetCoordinateInterface()->GetLocalToWorld();
if (!CompareMatrices(l2w, fCore->GetLastGlobalLoc(), .0001f))
{
fCore->SetKinematicLoc(l2w);
fCore->SetGlobalLoc(l2w);
}
}
return;
}
if (!fCore->IsEnabled())
return;
bool gotGroundHit = fGroundHit;
fGroundHit = false;
fCore->SetPushingPhysical(nil);
fCore->SetFacingPushingPhysical( false);
plSceneObject* so = plSceneObject::ConvertNoRef(fOwner->ObjectIsLoaded());
if (so)
{
static const float kGravity = -32.f;
// If we've been moved since the last physics update (somebody warped us),
// update the physics before we apply velocity.
const hsMatrix44& l2w = so->GetCoordinateInterface()->GetLocalToWorld();
if (!CompareMatrices(l2w, fCore->GetLastGlobalLoc(), .0001f))
fCore->SetGlobalLoc(l2w);
// Convert our avatar relative velocity to subworld relative
if (!LinearVelocity.IsEmpty())
{
LinearVelocity = l2w * LinearVelocity;
const plCoordinateInterface* subworldCI = fCore->GetSubworldCI();
if (subworldCI)
LinearVelocity = subworldCI->GetWorldToLocal() * LinearVelocity;
}
// Add in gravity if the avatar's z velocity isn't being set explicitly
// (Add in a little fudge factor, since the animations usually add a
// tiny bit of z.)
if (hsABS(LinearVelocity.fZ) < 0.001f)
{
// Get our previous z velocity. If we're on the ground, clamp it to zero at
// the largest, so we won't launch into the air if we're running uphill.
float prevZVel = AchievedLinearVelocity.fZ;
if (IsOnGround())
prevZVel = hsMinimum(prevZVel, 0.f);
float grav = kGravity * delSecs;
// If our gravity contribution isn't high enough this frame, we won't
// report a collision even when standing on solid ground.
float maxGrav = -.001f / delSecs;
if (grav > maxGrav)
grav = maxGrav;
LinearVelocity.fZ = prevZVel + grav;
}
// If we're airborne and the velocity isn't set, use the velocity from
// the last frame so we maintain momentum.
if (!IsOnGround() && LinearVelocity.fX == 0.f && LinearVelocity.fY == 0.f)
{
LinearVelocity.fX = AchievedLinearVelocity.fX;
LinearVelocity.fY = AchievedLinearVelocity.fY;
}
//make terminal velocity equal to k. it is wrong but has been this way and
//don't want to break any puzzles. on top of that it will reduce tunneling behavior
if(LinearVelocity.fZ<kGravity)LinearVelocity.fZ=kGravity;
fCore->SetLinearVelocity(LinearVelocity);
// Scale the velocity to our actual step size (by default it's feet/sec)
hsVector3 vel(LinearVelocity.fX * delSecs, LinearVelocity.fY * delSecs, LinearVelocity.fZ * delSecs);
unsigned int colFlags = 0;
fGroundHit = false;
fFalseGround = false;
fContactNormals.Swap(fPrevSlidingNormals);
fContactNormals.SetCount(0);
fCore->Move(vel, collideFlags, colFlags);
ICheckForFalseGround();
//if(fReqMove2) fCore->Move2(vel);
/*If the Physx controller thinks we have a collision from below, need to make sure we
have at least have false ground, otherwise Autostepping can send us into the air, and we will some times
float/panic link. For some reason the NxControllerHitReport does not always send messages
regarding Controller contact with ground plane, but will (almost) always return NXCC_COLLISION_DOWN
with the move method.
*/
if((colFlags&kBottom ) &&(fGroundHit==false))
{
fFalseGround=true;
}
if(colFlags&kTop)
{
fHitHead=true;
//Did you hit your head on a dynamic?
//with Physx's wonderful controller hit report vs flags issues we need to actually sweep to see
std::multiset< plControllerSweepRecord > HitsDynamic;
uint32_t testFlag=1<<plSimDefs::kGroupDynamic;
hsPoint3 startPos;
hsPoint3 endPos;
fCore->GetPositionSim(startPos);
endPos= startPos + vel;
int NumObjsHit=fCore->SweepControllerPath(startPos, endPos, true, false, testFlag, HitsDynamic);
if(NumObjsHit>0)
{
for(std::multiset< plControllerSweepRecord >::iterator curObj= HitsDynamic.begin();
curObj!=HitsDynamic.end(); curObj++)
{
hsAssert(curObj->ObjHit,"We allegedly hit something, but there is no plasma physical associated with it");
if(curObj->ObjHit)
{//really we shouldn't have to check hitObj should be nil only if we miss, or the physX object
//doesn't have a user data associated with this either way this just shouldn't happen
hsVector3 hitObjVel;
curObj->ObjHit->GetLinearVelocitySim(hitObjVel);
hsVector3 relativevel=LinearVelocity-hitObjVel;
curObj->ObjHit->SetHitForce(relativevel * 10.0f * (*curObj).ObjHit->GetMass(), (*curObj).locHit);
}
}
HitsDynamic.clear();
}
}
}
}
void plWalkingStrategy::Apply(float delSecs)
{
hsVector3 velocity = fController->GetLinearVelocity();
hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();
// Add in gravity if the avatar's z velocity isn't being set explicitly
if (hsABS(velocity.fZ) < 0.001f)
{
// Get our previous z velocity. If we're on the ground, clamp it to zero at
// the largest, so we won't launch into the air if we're running uphill.
float prevZVel = achievedVelocity.fZ;
if (IsOnGround())
prevZVel = hsMinimum(prevZVel, 0.0f);
velocity.fZ = prevZVel + (kGravity * delSecs);
}
// If we're airborne and the velocity isn't set, use the velocity from
// the last frame so we maintain momentum.
if (!IsOnGround() && velocity.fX == 0.0f && velocity.fY == 0.0f)
{
velocity.fX = achievedVelocity.fX;
velocity.fY = achievedVelocity.fY;
}
if (!fGroundHit && fSlidingNormals.Count())
{
// We're not on solid ground, so we should be sliding against whatever
// we're hitting (like a rock cliff). Each vector in fSlidingNormals is
// the surface normal of a collision that's too steep to be ground, so
// we project our current velocity onto that plane and slide along the
// wall.
//
// Also, sometimes PhysX reports a bunch of collisions from the wall,
// but nothing from underneath (when there should be). So if we're not
// touching ground, we offset the avatar in the direction of the
// surface normal(s). This doesn't fix the issue 100%, but it's a hell
// of a lot better than nothing, and suitable duct tape until a future
// PhysX revision fixes the issue.
//
// Yes, there's room for optimization here if we care.
hsVector3 offset(0.0f, 0.0f, 0.0f);
for (int i = 0; i < fSlidingNormals.GetCount(); i++)
{
offset += fSlidingNormals[i];
hsVector3 velNorm = velocity;
if (velNorm.MagnitudeSquared() > 0.0f)
velNorm.Normalize();
if (velNorm * fSlidingNormals[i] < 0.0f)
{
hsVector3 proj = (velNorm % fSlidingNormals[i]) % fSlidingNormals[i];
if (velNorm * proj < 0.0f)
proj *= -1.0f;
velocity = velocity.Magnitude() * proj;
}
}
if (offset.MagnitudeSquared() > 0.0f)
{
// 5 ft/sec is roughly the speed we walk backwards.
// The higher the value, the less likely you'll trip
// the bug, and this seems reasonable.
offset.Normalize();
velocity += offset * 5.0f;
}
}
if (velocity.fZ < kTerminalVelocity)
velocity.fZ = kTerminalVelocity;
// Convert to a displacement vector
hsVector3 displacement = velocity * delSecs;
// Reset vars and move the controller
fController->SetPushingPhysical(nil);
fController->SetFacingPushingPhysical(false);
fGroundHit = fFalseGround = fHeadHit = false;
fSlidingNormals.SetCount(0);
unsigned int collideResults = 0;
unsigned int collideFlags = 1<<plSimDefs::kGroupStatic | 1<<plSimDefs::kGroupAvatarBlocker | 1<<plSimDefs::kGroupDynamic;
if (!fController->IsSeeking())
collideFlags |= (1<<plSimDefs::kGroupExcludeRegion);
fController->Move(displacement, collideFlags, collideResults);
if ((!fGroundHit) && (collideResults & kBottom))
fFalseGround = true;
if (collideResults & kTop)
fHeadHit = true;
}
hsVectorStream* plPhysXCooking::IMakePolytope(const plMaxMeshExtractor::NeutralMesh& inMesh)
{
hsBool success=0;
std::vector<hsPoint3> outCloud;
hsPoint3 offset;
int numPlanes=26;
float planeMax[26];
int indexMax[26];
hsPoint3 AABBMin(FLT_MAX,FLT_MAX,FLT_MAX);
hsPoint3 AABBMax(-FLT_MAX,-FLT_MAX,-FLT_MAX);
//prep
NxVec3* vectors = new NxVec3[26];
int curvec=0;
for(int xcomp= -1;xcomp<2;xcomp++)
{
for(int ycomp= -1;ycomp<2;ycomp++)
{
for(int zcomp= -1;zcomp<2;zcomp++)
{
if(!((xcomp==0)&&(ycomp==0)&&(zcomp==0)))
{
vectors[curvec].set((float)(xcomp),(float)(ycomp),(float)(zcomp));
vectors[curvec].normalize();
planeMax[curvec]=(-FLT_MAX);
//indexMax[curvec]=0;
curvec++;
}
}
}
}
/*
for(int i=0;i<26;i++)
{//make your max and mins
planeMax[i]=(-FLT_MAX);
}
*/
hsPoint3 centroid(0.0f,0.0f,0.0f);
for(int i=0;i<inMesh.fNumVerts;i++) centroid+=inMesh.fVerts[i];
centroid=centroid/(float)inMesh.fNumVerts;
//temp
NxVec3* nxLocs=new NxVec3[inMesh.fNumVerts];
NxVec3* nxLocs2=new NxVec3[inMesh.fNumVerts];
for(int i=0;i<inMesh.fNumVerts;i++)
{
hsPoint3 temppt=inMesh.fVerts[i] - centroid;
nxLocs[i]=plPXConvert::Point(temppt);
}
NxMat33 rot;
NxVec3 eigen;
PCA(nxLocs,inMesh.fNumVerts,rot);
NxMat33 invrot;
rot.getInverse(invrot);
for(int i=0; i<inMesh.fNumVerts;i++)
{
nxLocs2[i]=invrot*nxLocs[i];
}
for(int i=0;i<inMesh.fNumVerts;i++)
{
for(int plane=0;plane<26;plane++)
{
float dist=nxLocs2[i].dot(vectors[plane]);
if(dist>=planeMax[plane])
{
planeMax[plane]=dist;
indexMax[plane]=i;
}
}
}
for(int i=0;i<inMesh.fNumVerts;i++)
{
AABBMin.fX = hsMinimum(nxLocs2[i].x, AABBMin.fX);
AABBMin.fY = hsMinimum(nxLocs2[i].y, AABBMin.fY);
AABBMin.fZ = hsMinimum(nxLocs2[i].z, AABBMin.fZ);
AABBMax.fX = hsMaximum(nxLocs2[i].x, AABBMax.fX);
AABBMax.fY = hsMaximum(nxLocs2[i].y, AABBMax.fY);
AABBMax.fZ = hsMaximum(nxLocs2[i].z, AABBMax.fZ);
}
int resultingPoints=0;
for(int i=0;i<26;i++)
{
for(int j=0;j<26;j++)
{
for(int k=0;k<26;k++)
{
NxVec3 res;
if(ThreePlaneIntersect(vectors[i],nxLocs2[indexMax[i]],vectors[j],nxLocs2[indexMax[j]], vectors[k],nxLocs2[indexMax[k]],res))
{
//check it is within all slabs
bool within=true;
int curplane=0;
do
{
float intersecdist=res.dot(vectors[curplane]);
if((intersecdist-planeMax[curplane])>0.0001)
{
within=false;
}
curplane++;
}
while((curplane<26)&&within);
if(within)
// if((res.x>=AABBMin.fX)&&(res.x<=AABBMax.fX)&&
// (res.y>=AABBMin.fY)&&(res.y<=AABBMax.fY)&&
// (res.z>=AABBMin.fZ)&&(res.z<=AABBMax.fZ))
{
NxVec3 reverted;
reverted=rot*res;
reverted.x=reverted.x +centroid.fX;
reverted.y=reverted.y +centroid.fY;
reverted.z=reverted.z +centroid.fZ;
hsPoint3 out;
out=plPXConvert::Point(reverted);
outCloud.push_back(out);
}
}
}
}
}
//planes discovered
//this is'nt right
//cleanup
offset=centroid;
delete[] vectors;
hsPoint3* pointages=new hsPoint3[outCloud.size()];
for(int x=0;x<outCloud.size();x++)pointages[x]=outCloud[x];
hsVectorStream* vectorstrm;
vectorstrm= CookHull(outCloud.size(),pointages,true);
delete[] pointages;
delete[] nxLocs;
delete[] nxLocs2;
return vectorstrm;
}