// TODO: I'm sure there is a more efficient approach.
static void TransformNormalToShapeSpace(const PxMeshScale& meshScale, const PxVec3& nIn, PxVec3& nOut)
{
// Uniform scale makes this unnecessary
if (meshScale.scale.x == meshScale.scale.y &&
meshScale.scale.x == meshScale.scale.z)
{
nOut = nIn;
return;
}
PxMat33 R(meshScale.rotation);
PxMat33 vertex2ShapeSkew = R.getTranspose();
PxMat33 diagonal = PxMat33::createDiagonal(meshScale.scale);
vertex2ShapeSkew = vertex2ShapeSkew * diagonal;
vertex2ShapeSkew = vertex2ShapeSkew * R;
PxMat33 shape2VertexSkew = vertex2ShapeSkew.getInverse();
const PxVec3 tmp = shape2VertexSkew.transformTranspose(nIn);
const PxReal Denom = 1.0f / tmp.magnitude();
nOut = tmp * Denom;
}
void collideWithSphereNonContinuous(PxsParticleCollData& collData, const PxVec3& pos, const PxReal& radius,
const PxReal& proxRadius)
{
if(collData.localFlags & PXS_FLUID_COLL_FLAG_CC)
return; // Only apply discrete and proximity collisions if no continuous collisions was detected so far (for any colliding shape)
PxReal dist = pos.magnitude();
collData.localSurfaceNormal = pos;
if(dist < (radius + proxRadius))
{
if (dist != 0.0f)
collData.localSurfaceNormal *= (1.0f / dist);
else
collData.localSurfaceNormal = PxVec3(0);
// Push particle to surface such that the distance to the surface is equal to the collision radius
collData.localSurfacePos = collData.localSurfaceNormal * (radius + collData.restOffset);
collData.localFlags |= PXS_FLUID_COLL_FLAG_L_PROX;
if(dist < (radius + collData.restOffset))
collData.localFlags |= PXS_FLUID_COLL_FLAG_L_DC;
}
}
// PT: ray-capsule intersection code, originally from the old Magic Software library.
PxU32 Gu::intersectRayCapsuleInternal(const PxVec3& origin, const PxVec3& dir, const PxVec3& p0, const PxVec3& p1, float radius, PxReal s[2])
{
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
PxVec3 kW = p1 - p0;
const float fWLength = kW.magnitude();
if(fWLength!=0.0f)
kW /= fWLength;
// PT: if the capsule is in fact a sphere, switch back to dedicated sphere code.
// This is not just an optimization, the rest of the code fails otherwise.
if(fWLength<=1e-6f)
{
const float d0 = (origin - p0).magnitudeSquared();
const float d1 = (origin - p1).magnitudeSquared();
const float approxLength = (PxMax(d0, d1) + radius)*2.0f;
return PxU32(Gu::intersectRaySphere(origin, dir, approxLength, p0, radius, s[0]));
}
// generate orthonormal basis
PxVec3 kU(0.0f);
if (fWLength > 0.0f)
{
PxReal fInvLength;
if ( PxAbs(kW.x) >= PxAbs(kW.y) )
{
// W.x or W.z is the largest magnitude component, swap them
fInvLength = PxRecipSqrt(kW.x*kW.x + kW.z*kW.z);
kU.x = -kW.z*fInvLength;
kU.y = 0.0f;
kU.z = kW.x*fInvLength;
}
else
{
// W.y or W.z is the largest magnitude component, swap them
fInvLength = PxRecipSqrt(kW.y*kW.y + kW.z*kW.z);
kU.x = 0.0f;
kU.y = kW.z*fInvLength;
kU.z = -kW.y*fInvLength;
}
}
PxVec3 kV = kW.cross(kU);
kV.normalize(); // PT: fixed november, 24, 2004. This is a bug in Magic.
// compute intersection
PxVec3 kD(kU.dot(dir), kV.dot(dir), kW.dot(dir));
const float fDLength = kD.magnitude();
const float fInvDLength = fDLength!=0.0f ? 1.0f/fDLength : 0.0f;
kD *= fInvDLength;
const PxVec3 kDiff = origin - p0;
const PxVec3 kP(kU.dot(kDiff), kV.dot(kDiff), kW.dot(kDiff));
const PxReal fRadiusSqr = radius*radius;
// Is the velocity parallel to the capsule direction? (or zero)
if ( PxAbs(kD.z) >= 1.0f - PX_EPS_REAL || fDLength < PX_EPS_REAL )
{
const float fAxisDir = dir.dot(kW);
const PxReal fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
if ( fAxisDir < 0 && fDiscr >= 0.0f )
{
// Velocity anti-parallel to the capsule direction
const PxReal fRoot = PxSqrt(fDiscr);
s[0] = (kP.z + fRoot)*fInvDLength;
s[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
return 2;
}
else if ( fAxisDir > 0 && fDiscr >= 0.0f )
{
// Velocity parallel to the capsule direction
const PxReal fRoot = PxSqrt(fDiscr);
s[0] = -(kP.z + fRoot)*fInvDLength;
s[1] = (fWLength - kP.z + fRoot)*fInvDLength;
return 2;
}
else
{
// sphere heading wrong direction, or no velocity at all
return 0;
}
}
// test intersection with infinite cylinder
PxReal fA = kD.x*kD.x + kD.y*kD.y;
PxReal fB = kP.x*kD.x + kP.y*kD.y;
PxReal fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
PxReal fDiscr = fB*fB - fA*fC;
if ( fDiscr < 0.0f )
{
// line does not intersect infinite cylinder
return 0;
}
PxU32 iQuantity = 0;
if ( fDiscr > 0.0f )
{
// line intersects infinite cylinder in two places
const PxReal fRoot = PxSqrt(fDiscr);
const PxReal fInv = 1.0f/fA;
PxReal fT = (-fB - fRoot)*fInv;
PxReal fTmp = kP.z + fT*kD.z;
const float epsilon = 1e-3f; // PT: see TA35174
if ( fTmp >= -epsilon && fTmp <= fWLength+epsilon )
s[iQuantity++] = fT*fInvDLength;
fT = (-fB + fRoot)*fInv;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= -epsilon && fTmp <= fWLength+epsilon )
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
{
// line intersects capsule wall in two places
return 2;
}
}
else
{
// line is tangent to infinite cylinder
const PxReal fT = -fB/fA;
const PxReal fTmp = kP.z + fT*kD.z;
if ( 0.0f <= fTmp && fTmp <= fWLength )
{
s[0] = fT*fInvDLength;
return 1;
}
}
// test intersection with bottom hemisphere
// fA = 1
fB += kP.z*kD.z;
fC += kP.z*kP.z;
fDiscr = fB*fB - fC;
if ( fDiscr > 0.0f )
{
const PxReal fRoot = PxSqrt(fDiscr);
PxReal fT = -fB - fRoot;
PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
fT = -fB + fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
else if ( fDiscr == 0.0f )
{
const PxReal fT = -fB;
const PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
// test intersection with top hemisphere
// fA = 1
fB -= kD.z*fWLength;
fC += fWLength*(fWLength - 2.0f*kP.z);
fDiscr = fB*fB - fC;
if ( fDiscr > 0.0f )
{
const PxReal fRoot = PxSqrt(fDiscr);
PxReal fT = -fB - fRoot;
PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
fT = -fB + fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
else if ( fDiscr == 0.0f )
{
const PxReal fT = -fB;
const PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
return iQuantity;
}
bool Character::move(PxReal speed, bool jump, bool active )
{
if (mCurrentMotion == NULL)
return false;
if (mBlendCounter > 0)
mBlendCounter--;
if (mTargetMotion && (mBlendCounter == 0))
{
mBlendCounter = 0;
mCurrentMotion = mTargetMotion;
mFrameTime = 0.0f;
mTargetMotion = NULL;
}
PxU32 nbFrames = mCurrentMotion->mNbFrames;
PxReal distance = mCurrentMotion->mDistance;
PxReal frameDelta = 0.0f;
const PxReal angleLimitPerFrame = 3.0f;
if (jump)
{
frameDelta = 1.0f;
}
else if (active && (mBlendCounter == 0))
{
// compute target orientation
PxVec3 dir = mTargetPosition - mCharacterPose.p;
dir.y = 0.0f;
PxReal curDistance = dir.magnitude();
if (curDistance > 0.01f)
faceToward(dir, angleLimitPerFrame);
frameDelta = speed * PxReal(nbFrames) * (curDistance / distance);
}
else
{
frameDelta = 0;
}
mCharacterPose.p = mTargetPosition;
mFrameTime += frameDelta;
PxU32 frameNo = PxU32(mFrameTime);
if (frameNo >= nbFrames)
{
if (jump)
mFrameTime = PxReal(nbFrames) - 1.0f;
else
mFrameTime = 0.0f;
}
// compute pose of all the bones at current frame (results are used by both getFramePose and Skin)
computeFramePose();
return true;
}
void PxClothGeodesicTetherCookerImpl::createTetherData(const PxClothMeshDesc &desc)
{
mNumParticles = desc.points.count;
if (!desc.invMasses.data)
return;
// assemble points
mVertices.resize(mNumParticles);
mAttached.resize(mNumParticles);
PxStrideIterator<const PxVec3> pIt((const PxVec3*)desc.points.data, desc.points.stride);
PxStrideIterator<const PxReal> wIt((const PxReal*)desc.invMasses.data, desc.invMasses.stride);
for(PxU32 i=0; i<mNumParticles; ++i)
{
mVertices[i] = *pIt++;
mAttached[i] = PxU8(wIt.ptr() ? (*wIt++ == 0.0f) : 0);
}
// build triangle indices
if(desc.flags & PxMeshFlag::e16_BIT_INDICES)
gatherIndices<PxU16>(mIndices, desc.triangles, desc.quads);
else
gatherIndices<PxU32>(mIndices, desc.triangles, desc.quads);
// build vertex-triangle adjacencies
findVertTriNeighbors();
// build triangle-triangle adjacencies
mCookerStatus = findTriNeighbors();
if (mCookerStatus != 0)
return;
// build adjacent vertex list
shdfnd::Array<PxU32> valency(mNumParticles+1, 0);
shdfnd::Array<PxU32> adjacencies;
if(desc.flags & PxMeshFlag::e16_BIT_INDICES)
gatherAdjacencies<PxU16>(valency, adjacencies, desc.triangles, desc.quads);
else
gatherAdjacencies<PxU32>(valency, adjacencies, desc.triangles, desc.quads);
// build unique neighbors from adjacencies
shdfnd::Array<PxU32> mark(valency.size(), 0);
shdfnd::Array<PxU32> neighbors; neighbors.reserve(adjacencies.size());
for(PxU32 i=1, j=0; i<valency.size(); ++i)
{
for(; j<valency[i]; ++j)
{
PxU32 k = adjacencies[j];
if(mark[k] != i)
{
mark[k] = i;
neighbors.pushBack(k);
}
}
valency[i] = neighbors.size();
}
// create islands of attachment points
shdfnd::Array<PxU32> vertexIsland(mNumParticles);
shdfnd::Array<VertexDistanceCount> vertexIslandHeap;
// put all the attachments in heap
for (PxU32 i = 0; i < mNumParticles; ++i)
{
// we put each attached point with large distance so that
// we can prioritize things that are added during mesh traversal.
vertexIsland[i] = PxU32(-1);
if (mAttached[i])
vertexIslandHeap.pushBack(VertexDistanceCount((int)i, FLT_MAX, 0));
}
PxU32 attachedCnt = vertexIslandHeap.size();
// no attached vertices
if (vertexIslandHeap.empty())
return;
// identify islands of attached vertices
shdfnd::Array<PxU32> islandIndices;
shdfnd::Array<PxU32> islandFirst;
PxU32 islandCnt = 0;
PxU32 islandIndexCnt = 0;
islandIndices.reserve(attachedCnt);
islandFirst.reserve(attachedCnt+1);
// while the island heap is not empty
while (!vertexIslandHeap.empty())
{
// pop vi from heap
VertexDistanceCount vi = popHeap(vertexIslandHeap);
// new cluster
if (vertexIsland[(PxU32)vi.vertNr] == PxU32(-1))
{
islandFirst.pushBack(islandIndexCnt++);
vertexIsland[(PxU32)vi.vertNr] = islandCnt++;
vi.distance = 0;
islandIndices.pushBack((PxU32)vi.vertNr);
}
// for each adjacent vj that's not visited
const PxU32 begin = (PxU32)valency[(PxU32)vi.vertNr];
const PxU32 end = (PxU32)valency[PxU32(vi.vertNr + 1)];
for (PxU32 j = begin; j < end; ++j)
{
const PxU32 vj = neighbors[j];
// do not expand unattached vertices
if (!mAttached[vj])
continue;
// already visited
if (vertexIsland[vj] != PxU32(-1))
continue;
islandIndices.pushBack(vj);
islandIndexCnt++;
vertexIsland[vj] = vertexIsland[PxU32(vi.vertNr)];
pushHeap(vertexIslandHeap, VertexDistanceCount((int)vj, vi.distance + 1.0f, 0));
}
}
islandFirst.pushBack(islandIndexCnt);
PX_ASSERT(islandCnt == (islandFirst.size() - 1));
/////////////////////////////////////////////////////////
PxU32 bufferSize = mNumParticles * islandCnt;
PX_ASSERT(bufferSize > 0);
shdfnd::Array<float> vertexDistanceBuffer(bufferSize, PX_MAX_F32);
shdfnd::Array<PxU32> vertexParentBuffer(bufferSize, 0);
shdfnd::Array<VertexDistanceCount> vertexHeap;
// now process each island
for (PxU32 i = 0; i < islandCnt; i++)
{
vertexHeap.clear();
float* vertexDistance = &vertexDistanceBuffer[0] + (i * mNumParticles);
PxU32* vertexParent = &vertexParentBuffer[0] + (i * mNumParticles);
// initialize parent and distance
for (PxU32 j = 0; j < mNumParticles; ++j)
{
vertexParent[j] = j;
vertexDistance[j] = PX_MAX_F32;
}
// put all the attached vertices in this island to heap
const PxU32 beginIsland = islandFirst[i];
const PxU32 endIsland = islandFirst[i+1];
for (PxU32 j = beginIsland; j < endIsland; j++)
{
PxU32 vj = islandIndices[j];
vertexDistance[vj] = 0.0f;
vertexHeap.pushBack(VertexDistanceCount((int)vj, 0.0f, 0));
}
// no attached vertices in this island (error?)
PX_ASSERT(vertexHeap.empty() == false);
if (vertexHeap.empty())
continue;
// while heap is not empty
while (!vertexHeap.empty())
{
// pop vi from heap
VertexDistanceCount vi = popHeap(vertexHeap);
// obsolete entry ( we already found better distance)
if (vi.distance > vertexDistance[vi.vertNr])
continue;
// for each adjacent vj that's not visited
const PxI32 begin = (PxI32)valency[(PxU32)vi.vertNr];
const PxI32 end = (PxI32)valency[PxU32(vi.vertNr + 1)];
for (PxI32 j = begin; j < end; ++j)
{
const PxI32 vj = (PxI32)neighbors[(PxU32)j];
PxVec3 edge = mVertices[(PxU32)vj] - mVertices[(PxU32)vi.vertNr];
const PxF32 edgeLength = edge.magnitude();
float newDistance = vi.distance + edgeLength;
if (newDistance < vertexDistance[vj])
{
vertexDistance[vj] = newDistance;
vertexParent[vj] = vertexParent[vi.vertNr];
pushHeap(vertexHeap, VertexDistanceCount(vj, newDistance, 0));
}
}
}
}
const PxU32 maxTethersPerParticle = 4; // max tethers
const PxU32 nbTethersPerParticle = (islandCnt > maxTethersPerParticle) ? maxTethersPerParticle : islandCnt;
PxU32 nbTethers = nbTethersPerParticle * mNumParticles;
mTetherAnchors.resize(nbTethers);
mTetherLengths.resize(nbTethers);
// now process the parent and distance and add to fibers
for (PxU32 i = 0; i < mNumParticles; i++)
{
// we use the heap to sort out N-closest island
vertexHeap.clear();
for (PxU32 j = 0; j < islandCnt; j++)
{
int parent = (int)vertexParentBuffer[j * mNumParticles + i];
float edgeDistance = vertexDistanceBuffer[j * mNumParticles + i];
pushHeap(vertexHeap, VertexDistanceCount(parent, edgeDistance, 0));
}
// take out N-closest island from the heap
for (PxU32 j = 0; j < nbTethersPerParticle; j++)
{
VertexDistanceCount vi = popHeap(vertexHeap);
PxU32 parent = (PxU32)vi.vertNr;
float distance = 0.0f;
if (parent != i)
{
float euclideanDistance = (mVertices[i] - mVertices[parent]).magnitude();
float dijkstraDistance = vi.distance;
int errorCode = 0;
float geodesicDistance = computeGeodesicDistance(i,parent, errorCode);
if (errorCode < 0)
geodesicDistance = dijkstraDistance;
distance = PxMax(euclideanDistance, geodesicDistance);
}
PxU32 tetherLoc = j * mNumParticles + i;
mTetherAnchors[ tetherLoc ] = parent;
mTetherLengths[ tetherLoc ] = distance;
}
}
}
bool doRaycastCCD(PxShape* shape, PxTransform& newPose, PxVec3& newShapeCenter, const PxVec3& ccdWitness, const PxVec3& ccdWitnessOffset)
{
PxRigidDynamic* dyna = canDoCCD(shape);
if(!dyna)
return true;
bool updateCCDWitness = true;
const PxVec3 offset = newPose.p - newShapeCenter;
//printf("CCD0: %f | %f | %f\n", newShapeCenter.x, newShapeCenter.y, newShapeCenter.z);
const PxVec3& origin = ccdWitness;
// const PxVec3& dest = newPose.p;
const PxVec3& dest = newShapeCenter;
PxVec3 dir = dest - origin;
const PxReal length = dir.magnitude();
if(length!=0.0f)
{
dir /= length;
// Compute internal radius
// PxVec3 localCenter;
const PxReal internalRadius = computeInternalRadius(shape, dir, /*localCenter,*/ ccdWitnessOffset);
// Compute distance to impact
PxRaycastHit hit;
// if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getActiveScene(), origin + localCenter, dir, length, hit))
if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getScene(), origin, dir, length, hit))
{
#ifdef RAYCAST_CCD_PRINT_DEBUG
static int count=0;
printf("CCD hit %d\n", count++);
#endif
updateCCDWitness = false;
const PxReal radiusLimit = internalRadius * 0.75f;
if(hit.distance>radiusLimit)
{
// newPose.p = origin + dir * (hit.distance - radiusLimit);
newShapeCenter = origin + dir * (hit.distance - radiusLimit);
#ifdef RAYCAST_CCD_PRINT_DEBUG
printf(" Path0: %f | %f\n", hit.distance, radiusLimit);
#endif
}
else
{
// newPose.p = origin;
newShapeCenter = origin;
// newShapeCenter = origin + hit.normal * (radiusLimit - hit.distance);
#ifdef RAYCAST_CCD_PRINT_DEBUG
printf(" Path1: %f\n", hit.distance);
#endif
}
{
newPose.p = offset + newShapeCenter;
//newPose.p.y += 10.0f;
//printf("%f | %f | %f\n", newPose.p.x, newPose.p.y, newPose.p.z);
// dyna->setGlobalPose(newPose);
// newPose = actorGlobalPose * shapeLocalPose
// newPose * inverse(shapeLocalPose) = actorGlobalPose
const PxTransform shapeLocalPose = shape->getLocalPose();
const PxTransform inverseShapeLocalPose = shapeLocalPose.getInverse();
PxTransform newGlobalPose = newPose * inverseShapeLocalPose;
dyna->setGlobalPose(newGlobalPose);
//dyna->setGlobalPose(newPose);
//printf("%f | %f | %f\n", newGlobalPose.p.x, newGlobalPose.p.y, newGlobalPose.p.z);
//printf("%f | %f | %f\n", shapeLocalPose.p.x, shapeLocalPose.p.y, shapeLocalPose.p.z);
/*PX_INLINE PxTransform PxShapeExt::getGlobalPose(const PxShape& shape)
{
PxRigidActor& ra = shape.getActor();
return ra.getGlobalPose() * shape.getLocalPose();
}*/
const PxVec3 testShapeCenter = getShapeCenter(shape, ccdWitnessOffset);
float d = (testShapeCenter - newShapeCenter).magnitude();
//printf("%f\n", d);
//printf("CCD1: %f | %f | %f\n", testShapeCenter.x, testShapeCenter.y, testShapeCenter.z);
//dyna->clearForce(PxForceMode::eFORCE);
//dyna->clearForce(PxForceMode::eIMPULSE);
//dyna->setLinearVelocity(PxVec3(0)); // PT: this helps the CCT but stops small objects dead, which doesn't look great
}
}
}
return updateCCDWitness;
}
/**
@brief
@date 2014-01-05
*/
void RendererCompositionShape::FindMostCloseFace(
void *positions0, PxU32 positionStride0, void *normals0, PxU32 normalStride0,
PxU16 *indices0, PxU32 idx0Size,
void *positions1, PxU32 positionStride1, void *normals1, PxU32 normalStride1,
PxU16 *indices1, PxU32 idx1Size,
OUT std::pair<int,int> &closeFace0, OUT std::pair<int,int> &closeFace1,
OUT set<PxU16> &vtx0, OUT set<PxU16> &vtx1 )
{
int foundCount = 0;
set<int> checkV0, checkV1;
while (foundCount < 2)
{
float minLen = 100000.f;
int minFaceIdx0 = -1;
int minFaceIdx1 = -1;
// find most close face
for (PxU8 i=0; i<idx0Size; i+=3)
{
if (checkV0.find(i) != checkV0.end())
continue; // already exist face index
PxVec3 center0;
PxVec3 center0Normal;
{
const PxU16 vidx0 = indices0[ i];
const PxU16 vidx1 = indices0[ i+1];
const PxU16 vidx2 = indices0[ i+2];
PxVec3 &p0 = *(PxVec3*)(((PxU8*)positions0) + (positionStride0 * vidx0));
PxVec3 &p1 = *(PxVec3*)(((PxU8*)positions0) + (positionStride0 * vidx1));
PxVec3 &p2 = *(PxVec3*)(((PxU8*)positions0) + (positionStride0 * vidx2));
PxVec3 &n0 = *(PxVec3*)(((PxU8*)normals0) + (normalStride0 * vidx0));
PxVec3 &n1 = *(PxVec3*)(((PxU8*)normals0) + (normalStride0 * vidx1));
PxVec3 &n2 = *(PxVec3*)(((PxU8*)normals0) + (normalStride0 * vidx2));
center0 = p0 + p1 + p2;
center0 /= 3.f;
center0Normal = n0;
}
for (PxU8 k=0; k<idx1Size; k+=3)
{
if (checkV1.find(k) != checkV1.end())
continue; // already exist face index
PxVec3 center1;
PxVec3 center1Normal;
{
const PxU16 vidx0 = indices1[ k];
const PxU16 vidx1 = indices1[ k+1];
const PxU16 vidx2 = indices1[ k+2];
PxVec3 &p0 = *(PxVec3*)(((PxU8*)positions1) + (positionStride1 * vidx0)) + PxVec3(1,0,0);
PxVec3 &p1 = *(PxVec3*)(((PxU8*)positions1) + (positionStride1 * vidx1)) + PxVec3(1,0,0);
PxVec3 &p2 = *(PxVec3*)(((PxU8*)positions1) + (positionStride1 * vidx2)) + PxVec3(1,0,0);
PxVec3 &n0 = *(PxVec3*)(((PxU8*)normals1) + (normalStride1 * vidx0));
PxVec3 &n1 = *(PxVec3*)(((PxU8*)normals1) + (normalStride1 * vidx1));
PxVec3 &n2 = *(PxVec3*)(((PxU8*)normals1) + (normalStride1 * vidx2));
center1 = p0 + p1 + p2;
center1 /= 3.f;
center1Normal = n0;
}
PxVec3 len = center0 - center1;
if ((minLen > len.magnitude()) && (center0Normal.dot(center1Normal) < 0) )
{
minFaceIdx0 = i;
minFaceIdx1 = k;
minLen = len.magnitude();
}
}
}
checkV0.insert(minFaceIdx0);
checkV1.insert(minFaceIdx1);
if (foundCount == 0)
{
closeFace0 = std::pair<int,int>(minFaceIdx0, minFaceIdx1);
}
else
{
closeFace1 = std::pair<int,int>(minFaceIdx0, minFaceIdx1);
}
++foundCount;
}
vtx0.insert( indices0[ closeFace0.first] );
vtx0.insert( indices0[ closeFace0.first+1] );
vtx0.insert( indices0[ closeFace0.first+2] );
vtx0.insert( indices0[ closeFace1.first] );
vtx0.insert( indices0[ closeFace1.first+1] );
vtx0.insert( indices0[ closeFace1.first+2] );
vtx1.insert( indices1[ closeFace0.second] );
vtx1.insert( indices1[ closeFace0.second+1] );
vtx1.insert( indices1[ closeFace0.second+2] );
vtx1.insert( indices1[ closeFace1.second] );
vtx1.insert( indices1[ closeFace1.second+1] );
vtx1.insert( indices1[ closeFace1.second+2] );
}
/**
@brief Mouse Move
@date 2014-03-04
*/
void COrientationEditController::MouseMove(physx::PxU32 x, physx::PxU32 y)
{
switch (m_editMode)
{
case MODE_NONE:
break;
case MODE_POSITION:
{
RET(!m_selectNode);
using namespace genotype_parser;
//const PxTransform tm = GetJointTransformAccumulate(m_rootNode, m_selectNode);
const SConnection *joint = m_rootNode->GetJoint(m_selectNode);
BRK(!joint);
const PxVec3 initialPos = utility::Vec3toPxVec3(joint->pos);
const PxVec3 dimension = utility::Vec3toPxVec3(m_selectNode->m_expr->dimension);
const float len = initialPos.magnitude() + dimension.magnitude();
PxVec3 orig, dir, pickOrig;
m_sample.GetPicking()->computeCameraRay(orig, dir, pickOrig, x, y);
PxVec3 pos = pickOrig + (dir*3);
PxVec3 v = pos - m_rootNode->GetPos();
if (v.magnitude() > len)
{
v.normalize();
pos = m_rootNode->GetPos() + (v*len);
}
m_selectNode->SetWorldTransform(PxTransform(pos));
//m_selectNode->m_renderNode->setTransform(PxTransform(pos));
}
break;
case MODE_PICKUP:
PickupNodes(m_sample, m_nodes, x, y, true);
break;
case MODE_ROTATION:
{
PxVec3 v0;
const Int2 pos(x,y);
if (pos == m_ptOrig)
break;
//const Int2 diff = pos - m_ptOrig;
{
PxVec3 orig0, dir0, pickOrig0;
m_sample.GetPicking()->computeCameraRay(orig0, dir0, pickOrig0, x, y);
PxVec3 orig1, dir1, pickOrig1;
m_sample.GetPicking()->computeCameraRay(orig1, dir1, pickOrig1, m_ptOrig.x, m_ptOrig.y);
PxVec3 v1 = pickOrig0 - pickOrig1;
v1.normalize();
v0 = dir0.cross(v1);
v0.normalize();
}
m_ptOrig = pos;
PxTransform tm = m_selectNode->GetLocalTransform();
PxTransform m = PxTransform(PxQuat(-0.03f, v0)) * tm;
m_selectNode->SetLocalTransform(m);
m_selectNode->UpdateTransform();
//PxQuat q0(diff.x*0.05f, PxVec3(0,1,0));
//PxQuat q1(-diff.y*0.05f, PxVec3(1,0,0));
//PxTransform m = tm * PxTransform(q1) * PxTransform(q0);
//m_selectNode->m_renderNode->setTransform(m);
}
break;
}
if (m_cursor)
m_cursor->MouseMove(x,y);
}
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;
}
/**
@brief MouseRButtonUp
@date 2014-03-04
*/
void COrientationEditController::MouseRButtonUp(physx::PxU32 x, physx::PxU32 y)
{
switch (m_editMode)
{
case MODE_POSITION:
{
RET(!m_selectNode);
using namespace genotype_parser;
SConnection *joint = m_rootNode->GetJoint(m_selectNode);
RET(!joint);
PxVec3 dir = m_selectNode->GetPos() - m_rootNode->GetPos();
const float len = dir.magnitude();
dir.normalize();
PxQuat rotateToXAxis;
if (boost::iequals(joint->type, "revolute"))
{
PxVec3 jointAxis = PxVec3(0,0,1).cross(dir);
jointAxis.normalize();
utility::quatRotationArc(rotateToXAxis, PxVec3(1,0,0), jointAxis);
}
else
{
rotateToXAxis = m_rootNode->GetWorldTransform().q;
}
{ // setting parent orientation
PxReal angle;
PxVec3 axis;
rotateToXAxis.toRadiansAndUnitAxis(angle, axis);
joint->parentOrient.angle = angle;
joint->parentOrient.dir = utility::PxVec3toVec3(axis);
printf( "parent angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
}
{ // setting select orientation
PxVec3 pos;
PxTransform rotTm;
const PxTransform tm = m_selectNode->GetWorldTransform();
if (boost::iequals(joint->type, "revolute"))
{
rotTm = PxTransform(rotateToXAxis) * PxTransform(tm.q);
pos = rotTm.rotate(dir*len);
}
else
{
rotTm = PxTransform(tm.q);
pos = tm.p;
}
PxReal angle;
PxVec3 axis;
rotTm.q.toRadiansAndUnitAxis(angle, axis);
joint->orient.angle = angle;
joint->orient.dir = utility::PxVec3toVec3(axis);
joint->pos = utility::PxVec3toVec3(pos);
printf( "connect angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
}
SelectNode(NULL);
}
break;
}
if (m_cursor)
m_cursor->MouseRButtonUp(x,y);
}
void DeformableMesh::calculateInvMasses(Ps::Array<PxReal>& invMasses, PxReal mass) const
{
PX_ASSERT(mVertexMasses.empty());
PX_ASSERT(invMasses.size() == mVertexPositions.size());
// clear output array
for (PxU32 i = 0; i < invMasses.size(); i++)
invMasses[i] = 0.0f;
switch (mPrimitiveType)
{
case PxDeformablePrimitiveType::eTRIANGLE:
{
// first, we compute area associated with each vertex and temporarily store it in invMasses array
PxReal totalArea = 0.0f;
const PxU32* i0 = mPrimitives.begin();
for (PxU32 i = 0; i < mPrimitives.size() / 3; ++i)
{
const PxU32* i1 = i0 + 1;
const PxU32* i2 = i0 + 2;
PxVec3 d0(mVertexPositions[*i0].x, mVertexPositions[*i0].y, mVertexPositions[*i0].z);
PxVec3 d1(mVertexPositions[*i1].x, mVertexPositions[*i1].y, mVertexPositions[*i1].z);
PxVec3 d2(mVertexPositions[*i2].x, mVertexPositions[*i2].y, mVertexPositions[*i2].z);
d1 = d1 - d0;
d2 = d2 - d0;
PxVec3 n = d1.cross(d2);
PxReal area = 0.5f * n.magnitude();
invMasses[*i0] += area / 3.0f;
invMasses[*i1] += area / 3.0f;
invMasses[*i2] += area / 3.0f;
totalArea += area;
i0 += 3;
}
// distribute overall mass by associated area
for (PxU32 i = 0; i < invMasses.size(); i++)
{
PxReal area = invMasses[i];
invMasses[i] = (1.0f / mass) * (totalArea / area);
}
}
break;
case PxDeformablePrimitiveType::eTETRAHEDRON:
{
// first, we compute volume associated with each vertex and temporarily store it in invMasses array
PxReal totalVolume = 0.0f;
const PxU32* i0 = mPrimitives.begin();
for (PxU32 i = 0; i < mPrimitives.size() / 4; i++)
{
const PxU32 *i1 = i0 + 1;
const PxU32 *i2 = i0 + 2;
const PxU32 *i3 = i0 + 3;
PxVec3 d0(mVertexPositions[*i0].x, mVertexPositions[*i0].y, mVertexPositions[*i0].z);
PxVec3 d1(mVertexPositions[*i1].x, mVertexPositions[*i1].y, mVertexPositions[*i1].z);
PxVec3 d2(mVertexPositions[*i2].x, mVertexPositions[*i2].y, mVertexPositions[*i2].z);
PxVec3 d3(mVertexPositions[*i3].x, mVertexPositions[*i3].y, mVertexPositions[*i3].z);
d1 = d1 - d0;
d2 = d2 - d0;
d3 = d3 - d0;
PxVec3 n = d1.cross(d2);
PxReal volume = n.dot(d3) / 6.0f;
invMasses[*i0] += volume / 4.0f;
invMasses[*i1] += volume / 4.0f;
invMasses[*i2] += volume / 4.0f;
invMasses[*i3] += volume / 4.0f;
totalVolume += volume;
i0 += 4;
}
// distribute overall mass by associated volume
for (PxU32 i = 0; i < invMasses.size(); i++)
{
PxReal volume = invMasses[i];
invMasses[i] = (1.0f / mass) * (totalVolume / volume);
}
}
break;
default:
PX_ASSERT(0);
break;
}
}
void DeformableMesh::generateConstraintsFromTetrahedra()
{
PxU32 edgeIndices[6][2] = { {0,1}, {0,2}, {0,3}, {1,2}, {1,3}, {2,3} };
PxU32 *tetIndices;
// - tetrahedra are assumed to be unique (thus no parent code here)
PxU32 numTetrahedra = mPrimitives.size() / 4;
DeformableTetraEdge e;
Ps::Array<DeformableTetraEdge> edges PX_DEBUG_EXP("defoMeshEdges2");
edges.reserve(numTetrahedra * 6);
tetIndices = mPrimitives.begin();
PxU32 i, j;
for (i = 0; i < numTetrahedra; i++, tetIndices += 4)
{
for(j = 0; j < 6; j++)
{
PxU32 e0 = mVertexToParticleMap[tetIndices[edgeIndices[j][0]]];
PxU32 e1 = mVertexToParticleMap[tetIndices[edgeIndices[j][1]]];
e.set(e0, e1, i); edges.pushBack(e);
}
}
quickSortTetraEdges(edges, 0, edges.size()-1);
mConstraints.resize(numTetrahedra);
DeformableConstraint constraint;
DeformableTetraEdge *tetEdges[6];
tetIndices = mPrimitives.begin();
bool warningIssued = false;
for (i = 0; i < numTetrahedra; i++, tetIndices += 4)
{
for (j = 0; j < 6; j++)
{
PxU32 e0 = mVertexToParticleMap[tetIndices[edgeIndices[j][0]]];
PxU32 e1 = mVertexToParticleMap[tetIndices[edgeIndices[j][1]]];
DeformableTetraEdge goalEdge;
goalEdge.set(e0, e1, i);
tetEdges[j] = binarySearchTetraEdge(edges, goalEdge);
}
for (j = 0; j < 4; j++)
constraint.particleId[j] = mVertexToParticleMap[tetIndices[j]];
PxVec3 groundArea = (mWeldedVertices[tetIndices[1]] - mWeldedVertices[tetIndices[0]]).cross(mWeldedVertices[tetIndices[2]] - mWeldedVertices[tetIndices[0]]);
constraint.restVolume = groundArea.dot(mWeldedVertices[tetIndices[3]] - mWeldedVertices[tetIndices[0]]);
constraint.flags = 0;
if (!warningIssued && constraint.restVolume < 0.0f)
{
Ps::getFoundation().error(PxErrorCode::eDEBUG_WARNING, __FILE__, __LINE__, "Soft body mesh tetrahedron %d has illegal winding order.", i);
warningIssued = true;
}
for (j = 0; j < 6; j++)
{
PxU32 e0 = mVertexToParticleMap[tetIndices[edgeIndices[j][0]]];
PxU32 e1 = mVertexToParticleMap[tetIndices[edgeIndices[j][1]]];
PxVec3 edgeVec = mWeldedVertices[e1] - mWeldedVertices[e0];
if(tetEdges[j])
{
PX_ASSERT(tetEdges[j]->tetrahedron == i);
constraint.restEdgeLengths[j] = edgeVec.magnitude();
}
else
constraint.restEdgeLengths[j] = -edgeVec.magnitude();
}
mConstraints[i] = constraint;
}
}
void RenderPhysX3Debug::addConeExt(float r0, float r1, const PxVec3& p0, const PxVec3& p1 , const RendererColor& color, PxU32 renderFlags)
{
PxVec3 axis = p1 - p0;
PxReal length = axis.magnitude();
PxReal rdiff = r0 - r1;
PxReal sinAngle = rdiff / length;
PxReal x0 = r0 * sinAngle;
PxReal x1 = r1 * sinAngle;
PxVec3 center = 0.5f * (p0 + p1);
if (length < fabs(rdiff))
return;
PxReal r0p = sqrt(r0 * r0 - x0 * x0);
PxReal r1p = sqrt(r1 * r1 - x1 * x1);
if (length == 0.0f)
axis = PxVec3(1,0,0);
else
axis.normalize();
PxVec3 axis1(0.0f);
axis1[minArgument(abs(axis))] = 1.0f;
axis1 = axis1.cross(axis);
axis1.normalize();
PxVec3 axis2 = axis.cross(axis1);
axis2.normalize();
PxMat44 m;
m.column0 = PxVec4(axis, 0.0f);
m.column1 = PxVec4(axis1, 0.0f);
m.column2 = PxVec4(axis2, 0.0f);
m.column3 = PxVec4(center, 1.0f);
PxTransform tr(m);
#define NUM_CONE_VERTS 72
const PxU32 nbVerts = NUM_CONE_VERTS;
PxVec3 pts0[NUM_CONE_VERTS] ;
PxVec3 pts1[NUM_CONE_VERTS];
PxVec3 normals[NUM_CONE_VERTS] ;
const float step = PxTwoPi / float(nbVerts);
for (PxU32 i = 0; i < nbVerts; i++)
{
const float angle = float(i) * step;
const float x = cosf(angle);
const float y = sinf(angle);
PxVec3 p = PxVec3(0.0f, x, y);
pts0[i] = tr.transform(r0p * p + PxVec3(-0.5f * length + x0,0,0));
pts1[i] = tr.transform(r1p * p + PxVec3(0.5f * length + x1, 0, 0));
normals[i] = tr.q.rotate(p.getNormalized());
normals[i] = x0 * axis + r0p * normals[i];
normals[i].normalize();
}
#undef NUM_CONE_VERTS
if(renderFlags & RENDER_DEBUG_WIREFRAME)
{
for(PxU32 i=0;i<nbVerts;i++)
{
addLine(pts1[i], pts0[i], color);
}
}
if(renderFlags & RENDER_DEBUG_SOLID)
{
for(PxU32 i=0;i<nbVerts;i++)
{
const PxU32 j = (i+1) % nbVerts;
addTriangle(pts0[i], pts1[j], pts0[j], normals[i], normals[j], normals[j], color);
addTriangle(pts0[i], pts1[i], pts1[j], normals[i], normals[i], normals[j], color);
}
}
}
const BoxPadded* cullBox) // Cull data
{
if(!nbTris)
return false;
const bool meshBothSides = hitFlags & PxHitFlag::eMESH_BOTH_SIDES;
const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;
const bool anyHit = hitFlags & PxHitFlag::eMESH_ANY;
const bool testInitialOverlap = !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP);
// PT: we can fallback to sphere sweep:
// - if the capsule is degenerate (i.e. it's a sphere)
// - if the sweep direction is the same as the capsule axis, in which case we can just sweep the top or bottom sphere
const PxVec3 extrusionDir = (capsule.p0 - capsule.p1)*0.5f; // Extrusion dir = capsule segment
const PxReal halfHeight = extrusionDir.magnitude();
bool mustExtrude = halfHeight!=0.0f;
if(!mustExtrude)
{
// PT: capsule is a sphere. Switch to sphere path (intersectCapsuleTriangle doesn't work for degenerate capsules)
return sweepSphereTriangles(nbTris, triangles, capsule.p0, capsule.radius, unitDir, distance, cachedIndex, hit, triNormalOut, isDoubleSided, meshBothSides, anyHit, testInitialOverlap);
}
else
{
const PxVec3 capsuleAxis = extrusionDir/halfHeight;
const PxReal colinearity = PxAbs(capsuleAxis.dot(unitDir));
mustExtrude = (colinearity < (1.0f - COLINEARITY_EPSILON));
}
const PxVec3 capsuleCenter = capsule.computeCenter();
