static FVector FindConvexMeshOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
if (IsInvalidFaceIndex(PHit.faceIndex))
{
return InNormal;
}
PxConvexMeshGeometry PConvexMeshGeom;
bool bSuccess = PHit.shape->getConvexMeshGeometry(PConvexMeshGeom);
check(bSuccess); //should only call this function when we have a convex mesh
if (PConvexMeshGeom.convexMesh)
{
check(PHit.faceIndex < PConvexMeshGeom.convexMesh->getNbPolygons());
const PxU32 PolyIndex = PHit.faceIndex;
PxHullPolygon PPoly;
bool bSuccessData = PConvexMeshGeom.convexMesh->getPolygonData(PolyIndex, PPoly);
if (bSuccessData)
{
// Account for non-uniform scale in local space normal.
const PxVec3 PPlaneNormal(PPoly.mPlane[0], PPoly.mPlane[1], PPoly.mPlane[2]);
const PxVec3 PLocalPolyNormal = TransformNormalToShapeSpace(PConvexMeshGeom.scale, PPlaneNormal.getNormalized());
// Convert to world space
const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
const PxVec3 PWorldPolyNormal = PShapeWorldPose.rotate(PLocalPolyNormal);
const FVector OutNormal = P2UVector(PWorldPolyNormal);
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
if (!OutNormal.IsNormalized())
{
UE_LOG(LogPhysics, Warning, TEXT("Non-normalized Normal (Hit shape is ConvexMesh): %s (LocalPolyNormal:%s)"), *OutNormal.ToString(), *P2UVector(PLocalPolyNormal).ToString());
UE_LOG(LogPhysics, Warning, TEXT("WorldTransform \n: %s"), *P2UTransform(PShapeWorldPose).ToString());
}
#endif
return OutNormal;
}
}
return InNormal;
}
static PX_FORCE_INLINE Ps::IntBool planeBoxOverlap(const PxVec3& normal, PxReal d, const PxVec3& maxbox)
{
PxVec3 vmin,vmax;
if (normal.x>0.0f)
{
vmin.x = -maxbox.x;
vmax.x = maxbox.x;
}
else
{
vmin.x = maxbox.x;
vmax.x = -maxbox.x;
}
if (normal.y>0.0f)
{
vmin.y = -maxbox.y;
vmax.y = maxbox.y;
}
else
{
vmin.y = maxbox.y;
vmax.y = -maxbox.y;
}
if (normal.z>0.0f)
{
vmin.z = -maxbox.z;
vmax.z = maxbox.z;
}
else
{
vmin.z = maxbox.z;
vmax.z = -maxbox.z;
}
if( normal.dot(vmin) + d > 0.0f) return Ps::IntFalse;
if( normal.dot(vmax) + d >= 0.0f) return Ps::IntTrue;
return Ps::IntFalse;
}
void Controller::setUpDirectionInternal(const PxVec3& up)
{
PX_CHECK_MSG(up.isNormalized(), "CCT: up direction must be normalized");
if(mUserParams.mUpDirection==up)
return;
// const PxQuat q = Ps::computeQuatFromNormal(up);
const PxQuat q = Ps::rotationArc(PxVec3(1.0f, 0.0f, 0.0f), up);
mUserParams.mQuatFromUp = q;
mUserParams.mUpDirection = up;
// Update kinematic actor
if(mKineActor)
{
PxTransform pose = mKineActor->getGlobalPose();
pose.q = q;
mKineActor->setGlobalPose(pose);
}
}
static PxQuat rotationArc(const PxVec3& v0, const PxVec3& v1, bool& res)
{
PxVec3 _v0 = v0;
PxVec3 _v1 = v1;
_v0.normalize();
_v1.normalize();
float s = sqrtf((1.0f + (v0.dot(v1))) * 2.0f);
if(s<0.001f)
{
res = false;
return PxQuat::createIdentity();
}
PxVec3 p = (_v0.cross(_v1)) / s;
float w = s * 0.5f;
PxQuat q(p.x, p.y, p.z, w);
q.normalize();
res = true;
return q;
}
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;
}
}
/**
@brief setting control diagramnode
@date 2014-03-02
*/
void COrientationEditController::SetControlDiagram(CGenotypeNode *node)
{
RemoveGenotypeTree(m_sample, m_rootNode);
m_nodes.clear();
m_rootNode = NULL;
vector<CGenotypeNode*> nodes;
m_genotypeController.GetDiagramsLinkto(node, nodes);
if (nodes.empty())
return;
// Create Phenotype Node
CGenotypeNode *parentNode = nodes[ 0];
map<const genotype_parser::SExpr*, CGenotypeNode*> symbols;
const PxTransform identTm = PxTransform::createIdentity();
m_rootNode = CreatePhenotypeDiagram(identTm,identTm,identTm, parentNode->m_expr, symbols);
// Camera Setting
const PxVec3 parentPos = parentNode->GetWorldTransform().p;
PxVec3 dir = parentPos - node->GetWorldTransform().p;
dir.normalize();
PxVec3 left = PxVec3(0,1,0).cross(dir);
left.normalize();
PxVec3 camPos = node->GetWorldTransform().p + (left*3.f) + PxVec3(0,2.5f,0);
PxVec3 camDir = parentPos - camPos;
camDir.normalize();
camPos -= (camDir * .5f);
m_sample.getCamera().lookAt(camPos, parentPos);
const PxTransform viewTm = m_sample.getCamera().getViewMatrix();
m_camera->init(viewTm);
// select Orientation Control node
if (CGenotypeNode *selectNode = m_rootNode->GetConnectNode(node->m_name))
{
selectNode->SetHighLight(true);
SelectNode(selectNode);
}
}
bool PxFabricCookerImpl::cook(const PxClothMeshDesc& desc, PxVec3 gravity, bool useGeodesicTether)
{
if(!desc.isValid())
{
shdfnd::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"PxFabricCookerImpl::cook: desc.isValid() failed!");
return false;
}
gravity = gravity.getNormalized();
mNumParticles = desc.points.count;
// assemble points
shdfnd::Array<PxVec4> particles;
particles.reserve(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)
particles.pushBack(PxVec4(*pIt++, wIt.ptr() ? *wIt++ : 1.0f));
// 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();
}
// build map of unique edges and classify
shdfnd::HashMap<Pair, Edge> edges;
for(PxU32 i=0; i<mNumParticles; ++i)
{
PxReal wi = particles[i].w;
// iterate all neighbors
PxU32 jlast = valency[i+1];
for(PxU32 j=valency[i]; j<jlast; ++j)
{
// add 1-ring edge
PxU32 m = neighbors[j];
if(wi + particles[m].w > 0.0f)
edges[Pair(PxMin(i, m), PxMax(i, m))].classify();
// iterate all neighbors of neighbor
PxU32 klast = valency[m+1];
for(PxU32 k=valency[m]; k<klast; ++k)
{
PxU32 n = neighbors[k];
if(n != i && wi + particles[n].w > 0.0f)
{
// add 2-ring edge
edges[Pair(PxMin(i, n), PxMax(i, n))].classify(
particles[i], particles[m], particles[n]);
}
}
}
}
// copy classified edges to constraints array
// build histogram of constraints per vertex
shdfnd::Array<Entry> constraints;
constraints.reserve(edges.size());
valency.resize(0); valency.resize(mNumParticles+1, 0);
const PxReal sqrtHalf = PxSqrt(0.4f);
for(shdfnd::HashMap<Pair, Edge>::Iterator eIt = edges.getIterator(); !eIt.done(); ++eIt)
{
const Edge& edge = eIt->second;
const Pair& pair = eIt->first;
if((edge.mStretching + edge.mBending + edge.mShearing) > 0.0f)
{
PxClothFabricPhaseType::Enum type = PxClothFabricPhaseType::eINVALID;
if(edge.mBending > PxMax(edge.mStretching, edge.mShearing))
type = PxClothFabricPhaseType::eBENDING;
else if(edge.mShearing > PxMax(edge.mStretching, edge.mBending))
type = PxClothFabricPhaseType::eSHEARING;
else
{
PxVec4 diff = particles[pair.first]-particles[pair.second];
PxReal dot = gravity.dot(reinterpret_cast<const PxVec3&>(diff).getNormalized());
type = fabsf(dot) < sqrtHalf ? PxClothFabricPhaseType::eHORIZONTAL : PxClothFabricPhaseType::eVERTICAL;
}
++valency[pair.first];
++valency[pair.second];
constraints.pushBack(Entry(pair, type));
}
}
prefixSum(valency.begin(), valency.end(), valency.begin());
PxU32 numConstraints = constraints.size();
// build adjacent constraint list
adjacencies.resize(0); adjacencies.resize(valency.back(), 0);
for(PxU32 i=0; i<numConstraints; ++i)
{
adjacencies[--valency[constraints[i].first.first]] = i;
adjacencies[--valency[constraints[i].first.second]] = i;
}
shdfnd::Array<PxU32>::ConstIterator aFirst = adjacencies.begin();
shdfnd::Array<PxU32> colors(numConstraints, numConstraints); // constraint -> color, initialily not colored
mark.resize(0); mark.resize(numConstraints+1, PX_MAX_U32); // color -> constraint index
shdfnd::Array<PxU32> adjColorCount(numConstraints, 0); // # of neighbors that are already colored
shdfnd::Array<ConstraintGraphColorCount> constraintHeap;
constraintHeap.reserve(numConstraints); // set of constraints to color (added in edge distance order)
// Do graph coloring based on edge distance.
// For each constraint, we add its uncolored neighbors to the heap
// ,and we pick the constraint with most colored neighbors from the heap.
while (1)
{
PxU32 constraint = 0;
while ( (constraint < numConstraints) && (colors[constraint] != numConstraints))
constraint++; // start with the first uncolored constraint
if (constraint >= numConstraints)
break;
constraintHeap.clear();
pushHeap(constraintHeap, ConstraintGraphColorCount((int)constraint, (int)adjColorCount[constraint]));
PxClothFabricPhaseType::Enum type = constraints[constraint].second;
while (!constraintHeap.empty())
{
ConstraintGraphColorCount heapItem = popHeap(constraintHeap);
constraint = heapItem.constraint;
if (colors[constraint] != numConstraints)
continue; // skip if already colored
const Pair& pair = constraints[constraint].first;
for(PxU32 j=0; j<2; ++j)
{
PxU32 index = j ? pair.first : pair.second;
if(particles[index].w == 0.0f)
continue; // don't mark adjacent particles if attached
for(shdfnd::Array<PxU32>::ConstIterator aIt = aFirst + valency[index], aEnd = aFirst + valency[index+1]; aIt != aEnd; ++aIt)
{
PxU32 adjacentConstraint = *aIt;
if ((constraints[adjacentConstraint].second != type) || (adjacentConstraint == constraint))
continue;
mark[colors[adjacentConstraint]] = constraint;
++adjColorCount[adjacentConstraint];
pushHeap(constraintHeap, ConstraintGraphColorCount((int)adjacentConstraint, (int)adjColorCount[adjacentConstraint]));
}
}
// find smallest color with matching type
PxU32 color = 0;
while((color < mPhases.size() && mPhases[color].phaseType != type) || mark[color] == constraint)
++color;
// create a new color set
if(color == mPhases.size())
{
PxClothFabricPhase phase(type, mPhases.size());
mPhases.pushBack(phase);
mSets.pushBack(0);
}
colors[constraint] = color;
++mSets[color];
}
}
#if 0 // PX_DEBUG
printf("set[%u] = ", mSets.size());
for(PxU32 i=0; i<mSets.size(); ++i)
printf("%u ", mSets[i]);
#endif
prefixSum(mSets.begin(), mSets.end(), mSets.begin());
#if 0 // PX_DEBUG
printf(" = %u\n", mSets.back());
#endif
// write indices and rest lengths
// convert mSets to exclusive sum
PxU32 back = mSets.back();
mSets.pushBack(back);
mIndices.resize(numConstraints*2);
mRestvalues.resize(numConstraints);
for(PxU32 i=0; i<numConstraints; ++i)
{
PxU32 first = constraints[i].first.first;
PxU32 second = constraints[i].first.second;
PxU32 index = --mSets[colors[i]];
mIndices[2*index ] = first;
mIndices[2*index+1] = second;
PxVec4 diff = particles[second] - particles[first];
mRestvalues[index] = reinterpret_cast<
const PxVec3&>(diff).magnitude();
}
// reorder constraints and rest values for more efficient cache access (linear)
shdfnd::Array<PxU32> newIndices(mIndices.size());
shdfnd::Array<PxF32> newRestValues(mRestvalues.size());
// sort each constraint set in vertex order
for (PxU32 i=0; i < mSets.size()-1; ++i)
{
// create a re-ordering list
shdfnd::Array<PxU32> reorder(mSets[i+1]-mSets[i]);
for (PxU32 r=0; r < reorder.size(); ++r)
reorder[r] = r;
const PxU32 indicesOffset = mSets[i]*2;
const PxU32 restOffset = mSets[i];
ConstraintSorter predicate(&mIndices[indicesOffset]);
shdfnd::sort(&reorder[0], reorder.size(), predicate);
for (PxU32 r=0; r < reorder.size(); ++r)
{
newIndices[indicesOffset + r*2] = mIndices[indicesOffset + reorder[r]*2];
newIndices[indicesOffset + r*2+1] = mIndices[indicesOffset + reorder[r]*2+1];
newRestValues[restOffset + r] = mRestvalues[restOffset + reorder[r]];
}
}
mIndices = newIndices;
mRestvalues = newRestValues;
PX_ASSERT(mIndices.size() == mRestvalues.size()*2);
PX_ASSERT(mRestvalues.size() == mSets.back());
#if 0 // PX_DEBUG
for (PxU32 i = 1; i < mSets.size(); i++)
{
PxClothFabricPhase phase = mPhases[i-1];
printf("%d : type %d, size %d\n",
i-1, phase.phaseType, mSets[i] - mSets[i-1]);
}
#endif
if (useGeodesicTether)
{
PxClothGeodesicTetherCooker tetherCooker(desc);
if (tetherCooker.getCookerStatus() == 0)
{
PxU32 numTethersPerParticle = tetherCooker.getNbTethersPerParticle();
PxU32 tetherSize = mNumParticles * numTethersPerParticle;
mTetherAnchors.resize(tetherSize);
mTetherLengths.resize(tetherSize);
tetherCooker.getTetherData(mTetherAnchors.begin(), mTetherLengths.begin());
}
else
useGeodesicTether = false;
}
if (!useGeodesicTether)
{
PxClothSimpleTetherCooker tetherCooker(desc);
mTetherAnchors.resize(mNumParticles);
mTetherLengths.resize(mNumParticles);
tetherCooker.getTetherData(mTetherAnchors.begin(), mTetherLengths.begin());
}
return true;
}
void PxSetJointGlobalFrame(PxJoint& joint, const PxVec3* wsAnchor, const PxVec3* axisIn)
{
PxRigidActor* actors[2];
joint.getActors(actors[0], actors[1]);
PxTransform localPose[2];
for(PxU32 i=0; i<2; i++)
localPose[i] = PxTransform::createIdentity();
// 1) global anchor
if(wsAnchor)
{
//transform anchorPoint to local space
for(PxU32 i=0; i<2; i++)
localPose[i].p = actors[i] ? actors[i]->getGlobalPose().transformInv(*wsAnchor) : *wsAnchor;
}
// 2) global axis
if(axisIn)
{
PxVec3 localAxis[2], localNormal[2];
//find 2 orthogonal vectors.
//gotta do this in world space, if we choose them
//separately in local space they won't match up in worldspace.
PxVec3 axisw = *axisIn;
axisw.normalize();
PxVec3 normalw, binormalw;
::normalToTangents(axisw, binormalw, normalw);
//because axis is supposed to be the Z axis of a frame with the other two being X and Y, we need to negate
//Y to make the frame right handed. Note that the above call makes a right handed frame if we pass X --> Y,Z, so
//it need not be changed.
for(PxU32 i=0; i<2; i++)
{
if(actors[i])
{
const PxTransform& m = actors[i]->getGlobalPose();
PxMat33Legacy mM(m.q);
localAxis[i] = mM % axisw;
localNormal[i] = mM % normalw;
}
else
{
localAxis[i] = axisw;
localNormal[i] = normalw;
}
PxMat33Legacy rot;
rot.setColumn(0, localAxis[i]);
rot.setColumn(1, localNormal[i]);
rot.setColumn(2, localAxis[i].cross(localNormal[i]));
localPose[i].q = rot.toQuat();
localPose[i].q.normalize();
}
}
for(PxU32 i=0; i<2; i++)
joint.setLocalPose(static_cast<PxJointActorIndex::Enum>( i ), localPose[i]);
}
void GOCCharacterController::ReceiveMessage(Msg &msg)
{
GOComponent::ReceiveMessage(msg);
if (msg.typeID == GlobalMessageIDs::PHYSICS_SUBSTEP && !mFreezed)
{
GameObjectPtr owner = mOwnerGO.lock();
float time = msg.params.GetFloat("TIME");
Ogre::Vector3 finalDir = Ogre::Vector3(0,0,0);
Ogre::Vector3 userDir = owner->GetGlobalOrientation() * mDirection;
if (mActor.getPxActor()->isSleeping())
mActor.getPxActor()->wakeUp(); //Gravity fix
Ogre::Vector3 playerHalfSize = mDimensions * 0.5f;
PxTransform transform(OgrePhysX::toPx(owner->GetGlobalPosition()), OgrePhysX::toPx(owner->GetGlobalOrientation()));
transform.p.y += playerHalfSize.y;
//sweep filter data - only check against shapes with filter data DYNAMICBODY or STATICBODY
PxSceneQueryFilterData filterData;
filterData.data.word0 = CollisionGroups::DYNAMICBODY|CollisionGroups::STATICBODY;
filterData.flags = PxSceneQueryFilterFlag::eDYNAMIC|PxSceneQueryFilterFlag::eSTATIC;
//touches ground check
PxBoxGeometry playerGeometry(playerHalfSize.x, playerHalfSize.y+0.1f, playerHalfSize.z);
PxShape *outShape;
mTouchesGround = Main::Instance().GetPhysXScene()->getPxScene()->overlapAny(playerGeometry, transform, outShape, filterData);
//stair maxStepHeight
float maxStepHeight = 0.6f;
PxVec3 currPos = OgrePhysX::Convert::toPx(owner->GetGlobalPosition());
//feet capsule
PxBoxGeometry feetVolume(playerHalfSize.x, maxStepHeight*0.5f, playerHalfSize.z);
//body capsule
float bodyHeight = mDimensions.y-maxStepHeight;
PxBoxGeometry bodyVolume(playerHalfSize.x, bodyHeight*0.5f, playerHalfSize.z);
PxVec3 sweepDirection = OgrePhysX::toPx(userDir);
float userDirLength = sweepDirection.normalize();
/*
We perform two sweeps:
O ==> bodyHit?
| ==> bodyHit?
/ \ ==> feetHit?
If there are no hits character can walk in the desired direction.
If there is a feetHit but no bodyHit player can climb stairs (we add an y-Offset).
If there is a bodyHit the player can not move.
*/
PxSweepHit sweepHit;
transform.p.y = owner->GetGlobalPosition().y + maxStepHeight + bodyHeight*0.5f;
bool bodyHit = Main::Instance().GetPhysXScene()->getPxScene()->sweepSingle(bodyVolume, transform, sweepDirection, time*userDirLength, PxSceneQueryFlags(), sweepHit, filterData);
transform.p.y = owner->GetGlobalPosition().y + maxStepHeight*0.5f;
bool feetHit = Main::Instance().GetPhysXScene()->getPxScene()->sweepSingle(feetVolume, transform, sweepDirection, time*userDirLength, PxSceneQueryFlags(), sweepHit, filterData);
if (!bodyHit)
{
finalDir += userDir; //add player movement
if (feetHit)
finalDir += Ogre::Vector3(0,3,0); //climb stairs
}
if (finalDir != Ogre::Vector3(0,0,0))
mActor.getPxActor()->setGlobalPose(PxTransform(currPos + OgrePhysX::Convert::toPx(finalDir*time)));
if (mJumping && mTouchesGround && (timeGetTime() - mJumpStartTime > 400))
{
mJumping = false;
Msg jump_response;
jump_response.typeID = ObjectMessageIDs::END_JUMP;
BroadcastObjectMessage(jump_response);
}
}
if (msg.typeID == GlobalMessageIDs::PHYSICS_END && !mFreezed)
{
SetOwnerPosition(mActor.getGlobalPosition());
//SetOwnerOrientation(mActor->getGlobalOrientation());
}
if (msg.typeID == ObjectMessageIDs::UPDATE_CHARACTER_MOVEMENTSTATE)
{
mDirection = Ogre::Vector3(0,0,0);
int movementFlags = msg.params.GetInt("CharacterMovementState");
if (movementFlags & CharacterMovement::FORWARD) mDirection.z += 1;
if (movementFlags & CharacterMovement::BACKWARD) mDirection.z -= 1;
if (movementFlags & CharacterMovement::LEFT) mDirection.x += 1;
if (movementFlags & CharacterMovement::RIGHT) mDirection.x -= 1;
mDirection.normalise();
mDirection*=(mMovementSpeed*mSpeedFactor);
}
if (msg.typeID == ObjectMessageIDs::INPUT_START_JUMP)
{
if (mTouchesGround && !mJumping)
{
mJumping = true;
mJumpStartTime = timeGetTime();
mActor.getPxActor()->addForce(PxVec3(0, mActor.getPxActor()->getMass()*8, 0), PxForceMode::eIMPULSE);
Msg startJumpMsg;
startJumpMsg.typeID = ObjectMessageIDs::START_JUMP;
BroadcastObjectMessage(startJumpMsg);
}
}
if (msg.typeID == ObjectMessageIDs::CHARACTER_KILL)
{
mFreezed = true;
}
}
bool physx::PxMeshQuery::sweep( const PxVec3& unitDir, const PxReal maxDistance,
const PxGeometry& geom, const PxTransform& pose,
PxU32 triangleCount, const PxTriangle* triangles,
PxSweepHit& sweepHit, PxHitFlags hintFlags_,
const PxU32* cachedIndex, const PxReal inflation, bool doubleSided)
{
PX_SIMD_GUARD;
PX_CHECK_AND_RETURN_VAL(pose.isValid(), "Gu::GeometryQuery::sweep(): pose is not valid.", false);
PX_CHECK_AND_RETURN_VAL(unitDir.isFinite(), "Gu::GeometryQuery::sweep(): unitDir is not valid.", false);
PX_CHECK_AND_RETURN_VAL(PxIsFinite(maxDistance), "Gu::GeometryQuery::sweep(): distance is not valid.", false);
PX_CHECK_AND_RETURN_VAL(maxDistance > 0, "Gu::GeometryQuery::sweep(): sweep distance must be greater than 0.", false);
const PxReal distance = PxMin(maxDistance, PX_MAX_SWEEP_DISTANCE);
// PT: the doc says that validity flags are not used, but internally some functions still check them. So
// to make sure the code works even when no validity flags are passed, we set them all here.
const PxHitFlags hintFlags = hintFlags_ | PxHitFlag::ePOSITION|PxHitFlag::eNORMAL|PxHitFlag::eDISTANCE;
switch(geom.getType())
{
case PxGeometryType::eSPHERE:
{
const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);
// PT: TODO: technically this capsule with 0.0 half-height is invalid ("isValid" returns false)
const PxCapsuleGeometry capsuleGeom(sphereGeom.radius, 0.0f);
return SweepCapsuleTriangles( triangleCount, triangles, doubleSided, capsuleGeom, pose, unitDir, distance,
cachedIndex, sweepHit.position, sweepHit.normal, sweepHit.distance, sweepHit.faceIndex, inflation, hintFlags);
}
case PxGeometryType::eCAPSULE:
{
const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);
return SweepCapsuleTriangles( triangleCount, triangles, doubleSided, capsuleGeom, pose, unitDir, distance,
cachedIndex, sweepHit.position, sweepHit.normal, sweepHit.distance, sweepHit.faceIndex, inflation, hintFlags);
}
case PxGeometryType::eBOX:
{
const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);
if(!PX_IS_SPU && (hintFlags & PxHitFlag::ePRECISE_SWEEP))
{
return sweepCCTBoxTriangles(triangleCount, triangles, doubleSided, boxGeom, pose,
unitDir, distance, sweepHit.position, sweepHit.normal, sweepHit.distance,
sweepHit.faceIndex, cachedIndex, inflation, hintFlags);
}
else
{
return SweepBoxTriangles( triangleCount, triangles, doubleSided, boxGeom, pose,
unitDir, distance, sweepHit.position, sweepHit.normal, sweepHit.distance,
sweepHit.faceIndex, cachedIndex, inflation, hintFlags);
}
}
case PxGeometryType::ePLANE:
case PxGeometryType::eCONVEXMESH:
case PxGeometryType::eTRIANGLEMESH:
case PxGeometryType::eHEIGHTFIELD:
case PxGeometryType::eGEOMETRY_COUNT:
case PxGeometryType::eINVALID:
default :
PX_CHECK_MSG(false, "Gu::GeometryQuery::sweep(): geometry object parameter must be sphere, capsule or box geometry.");
}
return false;
}
/**
@brief vector 4개로 triangle 2개를 그리는 index buffer를 생성한다.
@date 2014-01-30
*/
void SampleRenderer::GenerateTriangleFrom4Vector2( void *positions, void *normals, void *bones, void *colors,
PxU32 stride, PxU32 startVtxIdx, PxU16 *indices, PxU32 startIndexIdx, const PxVec3 ¢er,
const vector<PxU16> &faceIndices, OUT vector<PxU16> &outfaceIndices )
{
vector<PxU16> triangle0; triangle0.reserve(3);
vector<PxU16> triangle1; triangle1.reserve(3);
triangle0.push_back( faceIndices[ 0] );
triangle0.push_back( faceIndices[ 2] );
triangle0.push_back( faceIndices[ 3] );
triangle1.push_back( faceIndices[ 0] );
triangle1.push_back( faceIndices[ 1] );
triangle1.push_back( faceIndices[ 3] );
GenerateTriangleFrom3Vector(positions, normals, stride, center, triangle0, outfaceIndices);
GenerateTriangleFrom3Vector(positions, normals, stride, center, triangle1, outfaceIndices);
if (outfaceIndices.size() != 6)
return; // error occur
for (u_int i=0; i < outfaceIndices.size();)
{
const PxVec3 p0 = *(PxVec3*)(((PxU8*)positions) + (stride * outfaceIndices[ i]));
const PxVec3 p1 = *(PxVec3*)(((PxU8*)positions) + (stride * outfaceIndices[ i+1]));
const PxVec3 p2 = *(PxVec3*)(((PxU8*)positions) + (stride * outfaceIndices[ i+2]));
*(PxVec3*)(((PxU8*)positions) + (stride * startVtxIdx)) = p0;
*(PxVec3*)(((PxU8*)positions) + (stride * (startVtxIdx+1))) = p1;
*(PxVec3*)(((PxU8*)positions) + (stride * (startVtxIdx+2))) = p2;
if (bones)
{
const PxU32 b0 = *(PxU32*)(((PxU8*)bones) + (stride * outfaceIndices[ i]));
const PxU32 b1 = *(PxU32*)(((PxU8*)bones) + (stride * outfaceIndices[ i+1]));
const PxU32 b2 = *(PxU32*)(((PxU8*)bones) + (stride * outfaceIndices[ i+2]));
*(PxU32*)(((PxU8*)bones) + (stride * startVtxIdx)) = b0;
*(PxU32*)(((PxU8*)bones) + (stride * (startVtxIdx+1))) = b1;
*(PxU32*)(((PxU8*)bones) + (stride * (startVtxIdx+2))) = b2;
}
if (colors)
{
const PxU32 c0 = *(PxU32*)(((PxU8*)colors) + (stride * outfaceIndices[ i]));
const PxU32 c1 = *(PxU32*)(((PxU8*)colors) + (stride * outfaceIndices[ i+1]));
const PxU32 c2 = *(PxU32*)(((PxU8*)colors) + (stride * outfaceIndices[ i+2]));
*(PxU32*)(((PxU8*)colors) + (stride * startVtxIdx)) = c0;
*(PxU32*)(((PxU8*)colors) + (stride * (startVtxIdx+1))) = c1;
*(PxU32*)(((PxU8*)colors) + (stride * (startVtxIdx+2))) = c2;
}
if (normals)
{
PxVec3 v01 = p1 - p0;
PxVec3 v02 = p2 - p0;
v01.normalize();
v02.normalize();
PxVec3 n = v01.cross(v02);
n.normalize();
n = -n;
*(PxVec3*)(((PxU8*)normals) + (stride * startVtxIdx)) = n;
*(PxVec3*)(((PxU8*)normals) + (stride * (startVtxIdx+1))) = n;
*(PxVec3*)(((PxU8*)normals) + (stride * (startVtxIdx+2))) = n;
}
indices[ startIndexIdx] = startVtxIdx;
indices[ startIndexIdx+1] = startVtxIdx+1;
indices[ startIndexIdx+2] = startVtxIdx+2;
startIndexIdx += 3;
startVtxIdx += 3;
i += 3;
}
}
PxVec3 RandomOrthogonalVector(PxVec3 normal)
{
PxVec3 random = CreateRandomVector(10);
return random - (normal * random.dot(normal));
}
PX_FORCE_INLINE void collideWithSphere(PxsParticleCollData& collData, const PxSphereGeometry& sphereShapeData,
PxReal proxRadius)
{
PxVec3& oldPos = collData.localOldPos;
PxVec3& newPos = collData.localNewPos;
PxReal radius = sphereShapeData.radius;
PxReal oldPosDist2 = oldPos.magnitudeSquared();
PxReal radius2 = radius * radius;
bool oldInSphere = (oldPosDist2 < radius2);
if(oldInSphere)
{
// old position inside the skeleton
// add ccd with time 0.0
collData.localSurfaceNormal = oldPos;
if (oldPosDist2 > 0.0f)
collData.localSurfaceNormal *= PxRecipSqrt(oldPosDist2);
else
collData.localSurfaceNormal = PxVec3(0,1.0f,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.ccTime = 0.0;
collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
}
else
{
// old position is outside of the skeleton
PxVec3 motion = newPos - oldPos;
// Discriminant
PxReal b = motion.dot(oldPos) * 2.0f;
PxReal a2 = 2.0f * motion.magnitudeSquared();
PxReal disc = (b*b) - (2.0f * a2 * (oldPosDist2 - radius2));
bool intersection = disc > 0.0f;
if ((!intersection) || (a2 == 0.0f))
{
// the ray does not intersect the sphere
collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
}
else
{
// the ray intersects the sphere
PxReal t = -(b + PxSqrt(disc)) / a2; // Compute intersection point
if (t < 0.0f || t > 1.0f)
{
// intersection point lies outside motion vector
collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
}
else if(t < collData.ccTime)
{
// intersection point lies on sphere, add lcc
//collData.localSurfacePos = oldPos + (motion * t);
//collData.localSurfaceNormal = collData.localSurfacePos;
//collData.localSurfaceNormal *= (1.0f / radius);
//collData.localSurfacePos += (collData.localSurfaceNormal * collData.restOffset);
PxVec3 relativeImpact = motion*t;
collData.localSurfaceNormal = oldPos + relativeImpact;
collData.localSurfaceNormal *= (1.0f / radius);
computeContinuousTargetPosition(collData.localSurfacePos, collData.localOldPos, relativeImpact, collData.localSurfaceNormal, collData.restOffset);
collData.ccTime = t;
collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
}
}
}
}
bool Gu::intersectOBBOBB(const PxVec3& e0, const PxVec3& c0, const PxMat33& r0, const PxVec3& e1, const PxVec3& c1, const PxMat33& r1, bool full_test)
{
// Translation, in parent frame
const PxVec3 v = c1 - c0;
// Translation, in A's frame
const PxVec3 T(v.dot(r0[0]), v.dot(r0[1]), v.dot(r0[2]));
// B's basis with respect to A's local frame
PxReal R[3][3];
PxReal FR[3][3];
PxReal ra, rb, t;
// Calculate rotation matrix
for(PxU32 i=0;i<3;i++)
{
for(PxU32 k=0;k<3;k++)
{
R[i][k] = r0[i].dot(r1[k]);
FR[i][k] = 1e-6f + PxAbs(R[i][k]); // Precompute fabs matrix
}
}
// A's basis vectors
for(PxU32 i=0;i<3;i++)
{
ra = e0[i];
rb = e1[0]*FR[i][0] + e1[1]*FR[i][1] + e1[2]*FR[i][2];
t = PxAbs(T[i]);
if(t > ra + rb) return false;
}
// B's basis vectors
for(PxU32 k=0;k<3;k++)
{
ra = e0[0]*FR[0][k] + e0[1]*FR[1][k] + e0[2]*FR[2][k];
rb = e1[k];
t = PxAbs(T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k]);
if( t > ra + rb ) return false;
}
if(full_test)
{
//9 cross products
//L = A0 x B0
ra = e0[1]*FR[2][0] + e0[2]*FR[1][0];
rb = e1[1]*FR[0][2] + e1[2]*FR[0][1];
t = PxAbs(T[2]*R[1][0] - T[1]*R[2][0]);
if(t > ra + rb) return false;
//L = A0 x B1
ra = e0[1]*FR[2][1] + e0[2]*FR[1][1];
rb = e1[0]*FR[0][2] + e1[2]*FR[0][0];
t = PxAbs(T[2]*R[1][1] - T[1]*R[2][1]);
if(t > ra + rb) return false;
//L = A0 x B2
ra = e0[1]*FR[2][2] + e0[2]*FR[1][2];
rb = e1[0]*FR[0][1] + e1[1]*FR[0][0];
t = PxAbs(T[2]*R[1][2] - T[1]*R[2][2]);
if(t > ra + rb) return false;
//L = A1 x B0
ra = e0[0]*FR[2][0] + e0[2]*FR[0][0];
rb = e1[1]*FR[1][2] + e1[2]*FR[1][1];
t = PxAbs(T[0]*R[2][0] - T[2]*R[0][0]);
if(t > ra + rb) return false;
//L = A1 x B1
ra = e0[0]*FR[2][1] + e0[2]*FR[0][1];
rb = e1[0]*FR[1][2] + e1[2]*FR[1][0];
t = PxAbs(T[0]*R[2][1] - T[2]*R[0][1]);
if(t > ra + rb) return false;
//L = A1 x B2
ra = e0[0]*FR[2][2] + e0[2]*FR[0][2];
rb = e1[0]*FR[1][1] + e1[1]*FR[1][0];
t = PxAbs(T[0]*R[2][2] - T[2]*R[0][2]);
if(t > ra + rb) return false;
//L = A2 x B0
ra = e0[0]*FR[1][0] + e0[1]*FR[0][0];
rb = e1[1]*FR[2][2] + e1[2]*FR[2][1];
t = PxAbs(T[1]*R[0][0] - T[0]*R[1][0]);
if(t > ra + rb) return false;
//L = A2 x B1
ra = e0[0]*FR[1][1] + e0[1]*FR[0][1];
rb = e1[0] *FR[2][2] + e1[2]*FR[2][0];
t = PxAbs(T[1]*R[0][1] - T[0]*R[1][1]);
if(t > ra + rb) return false;
//L = A2 x B2
ra = e0[0]*FR[1][2] + e0[1]*FR[0][2];
rb = e1[0]*FR[2][1] + e1[1]*FR[2][0];
t = PxAbs(T[1]*R[0][2] - T[0]*R[1][2]);
if(t > ra + rb) return false;
}
return true;
}
bool setupFinalizeExtSolverContactsCoulomb(
const ContactBuffer& buffer,
const CorrelationBuffer& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
PxU8* workspace,
PxReal invDt,
PxReal bounceThresholdF32,
const SolverExtBody& b0,
const SolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1,
PxReal restDist,
PxReal ccdMaxDistance)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
const FloatV ccdMaxSeparation = FLoad(ccdMaxDistance);
PxU8* PX_RESTRICT ptr = workspace;
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];
const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(restDist);
const FloatV bounceThreshold = FLoad(bounceThresholdF32);
const FloatV invDtV = FLoad(invDt);
const FloatV pt8 = FLoad(0.8f);
const FloatV invDtp8 = FMul(invDtV, pt8);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
const PxU32 frictionPatchCount = c.frictionPatchCount;
const PxU32 pointStride = sizeof(SolverContactPointExt);
const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
const PxU8 pointHeaderType = DY_SC_TYPE_EXT_CONTACT;
const PxU8 frictionHeaderType = DY_SC_TYPE_EXT_FRICTION;
PxReal d0 = invMassScale0;
PxReal d1 = invMassScale1;
PxReal angD0 = invInertiaScale0;
PxReal angD1 = invInertiaScale1;
PxU8 flags = 0;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const Vec3V normalV = Ps::aos::V3LoadA(contactBase0->normal);
const Vec3V normal = V3LoadA(contactBase0->normal);
const PxReal combinedRestitution = contactBase0->restitution;
SolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<SolverContactCoulombHeader*>(ptr);
ptr += sizeof(SolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const FloatV restitution = FLoad(combinedRestitution);
header->numNormalConstr = PxU8(contactCount);
header->type = pointHeaderType;
//header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->flags = flags;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
p += pointStride;
setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDtV, invDtp8, restDistance, maxPenBias, restitution,
bounceThreshold, contact, *solverContact, ccdMaxSeparation);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(SolverContactCoulombHeader) + header->numNormalConstr * pointStride;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
PxVec3 normal = contactBase0->normal;
const PxReal staticFriction = contactBase0->staticFriction;
const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
const bool haveFriction = (disableStrongFriction == 0);
SolverFrictionHeader* frictionHeader = reinterpret_cast<SolverFrictionHeader*>(ptr);
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatchContactCounts[i] * frictionCountPerPoint : 0);
frictionHeader->flags = flags;
ptr += sizeof(SolverFrictionHeader);
PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
PxMemZero(forceBuffer, sizeof(PxF32) * c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->invMass0D0 = d0;
frictionHeader->invMass1D1 = d1;
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
const PxVec3 targetVel = contact.targetVel;
const PxVec3 pVRa = b0.getLinVel() + b0.getAngVel().cross(ra);
const PxVec3 pVRb = b1.getLinVel() + b1.getAngVel().cross(rb);
//const PxVec3 vrel = pVRa - pVRb;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(p);
p += frictionStride;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t0, -rbXn, b1);
PxReal unitResponse = getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1);
PxReal tv = targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
tv += pVRa.dot(t0);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
tv -= pVRb.dot(t0);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(resp0.angular);
f0->setRbXn(-resp1.angular);
f0->targetVel = tv;
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->linDeltaVA = V3LoadA(deltaV0.linear);
f0->angDeltaVA = V3LoadA(deltaV0.angular);
f0->linDeltaVB = V3LoadA(deltaV1.linear);
f0->angDeltaVB = V3LoadA(deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
// this code is shared between capsule vertices and sphere
bool GuContactSphereHeightFieldShared(GU_CONTACT_METHOD_ARGS, bool isCapsule)
{
#if 1
PX_UNUSED(cache);
PX_UNUSED(renderOutput);
// Get actual shape data
const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
const PxHeightFieldGeometryLL& hfGeom = shape1.get<const PxHeightFieldGeometryLL>();
const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(hfGeom.heightField);
const Gu::HeightFieldUtil hfUtil(hfGeom, hf);
const PxReal radius = shapeSphere.radius;
const PxReal eps = PxReal(0.1) * radius;
const PxVec3 sphereInHfShape = transform1.transformInv(transform0.p);
PX_ASSERT(isCapsule || contactBuffer.count==0);
const PxReal oneOverRowScale = hfUtil.getOneOverRowScale();
const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale();
// check if the sphere is below the HF surface
if (hfUtil.isShapePointOnHeightField(sphereInHfShape.x, sphereInHfShape.z))
{
PxReal fracX, fracZ;
const PxU32 vertexIndex = hfUtil.getHeightField().computeCellCoordinates(sphereInHfShape.x * oneOverRowScale, sphereInHfShape.z * oneOverColumnScale, fracX, fracZ);
// The sphere origin projects within the bounds of the heightfield in the X-Z plane
// const PxReal sampleHeight = hfShape.getHeightAtShapePoint(sphereInHfShape.x, sphereInHfShape.z);
const PxReal sampleHeight = hfUtil.getHeightAtShapePoint2(vertexIndex, fracX, fracZ);
const PxReal deltaHeight = sphereInHfShape.y - sampleHeight;
//if (hfShape.isDeltaHeightInsideExtent(deltaHeight, eps))
if (hf.isDeltaHeightInsideExtent(deltaHeight, eps))
{
// The sphere origin is 'below' the heightfield surface
// Actually there is an epsilon involved to make sure the
// 'above' surface calculations can deliver a good normal
const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePointNoTest2(vertexIndex, fracX, fracZ);
if (faceIndex != 0xffffffff)
{
//hfShape.getAbsPoseFast().M.getColumn(1, hfShapeUp);
const PxVec3 hfShapeUp = transform1.q.getBasisVector1();
if (hf.getThicknessFast() <= 0)
contactBuffer.contact(transform0.p, hfShapeUp, deltaHeight-radius, faceIndex);
else
contactBuffer.contact(transform0.p, -hfShapeUp, -deltaHeight-radius, faceIndex);
return true;
}
return false;
}
}
const PxReal epsSqr = eps * eps;
const PxReal inflatedRadius = radius + params.mContactDistance;
const PxReal inflatedRadiusSquared = inflatedRadius * inflatedRadius;
const PxVec3 sphereInHF = hfUtil.shape2hfp(sphereInHfShape);
const PxReal radiusOverRowScale = inflatedRadius * PxAbs(oneOverRowScale);
const PxReal radiusOverColumnScale = inflatedRadius * PxAbs(oneOverColumnScale);
const PxU32 minRow = hf.getMinRow(sphereInHF.x - radiusOverRowScale);
const PxU32 maxRow = hf.getMaxRow(sphereInHF.x + radiusOverRowScale);
const PxU32 minColumn = hf.getMinColumn(sphereInHF.z - radiusOverColumnScale);
const PxU32 maxColumn = hf.getMaxColumn(sphereInHF.z + radiusOverColumnScale);
// this assert is here because the following code depends on it for reasonable performance for high-count situations
PX_COMPILE_TIME_ASSERT(ContactBuffer::MAX_CONTACTS == 64);
const PxU32 nbColumns = hf.getNbColumnsFast();
#define HFU Gu::HeightFieldUtil
PxU32 numFaceContacts = 0;
for (PxU32 i = 0; i<2; i++)
{
const bool facesOnly = (i == 0);
// first we go over faces-only meaning only contacts directly in Voronoi regions of faces
// at second pass we consider edges and vertices and clamp the normals to adjacent feature's normal
// if there was a prior contact. it is equivalent to clipping the normal to it's feature's Voronoi region
for (PxU32 r = minRow; r < maxRow; r++)
{
for (PxU32 c = minColumn; c < maxColumn; c++)
{
// x--x--x
// | x |
// x x x
// | x |
// x--x--x
PxVec3 closestPoints[11];
PxU32 closestFeatures[11];
PxU32 npcp = hfUtil.findClosestPointsOnCell(
r, c, sphereInHfShape, closestPoints, closestFeatures, facesOnly, !facesOnly, true);
for(PxU32 pi = 0; pi < npcp; pi++)
{
PX_ASSERT(closestFeatures[pi] != 0xffffffff);
const PxVec3 d = sphereInHfShape - closestPoints[pi];
if (hf.isDeltaHeightOppositeExtent(d.y)) // See if we are 'above' the heightfield
{
const PxReal dMagSq = d.magnitudeSquared();
if (dMagSq > inflatedRadiusSquared)
// Too far above
continue;
PxReal dMag = -1.0f; // dMag is sqrt(sMadSq) and comes up as a byproduct of other calculations in computePointNormal
PxVec3 n; // n is in world space, rotated by transform1
PxU32 featureType = HFU::getFeatureType(closestFeatures[pi]);
if (featureType == HFU::eEDGE)
{
PxU32 edgeIndex = HFU::getFeatureIndex(closestFeatures[pi]);
PxU32 adjFaceIndices[2];
const PxU32 adjFaceCount = hf.getEdgeTriangleIndices(edgeIndex, adjFaceIndices);
PxVec3 origin;
PxVec3 direction;
const PxU32 vertexIndex = edgeIndex / 3;
const PxU32 row = vertexIndex / nbColumns;
const PxU32 col = vertexIndex % nbColumns;
hfUtil.getEdge(edgeIndex, vertexIndex, row, col, origin, direction);
n = hfUtil.computePointNormal(
hfGeom.heightFieldFlags, d, transform1, dMagSq,
closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
PxVec3 localN = transform1.rotateInv(n);
// clamp the edge's normal to its Voronoi region
for (PxU32 j = 0; j < adjFaceCount; j++)
{
const PxVec3 adjNormal = hfUtil.hf2shapen(hf.getTriangleNormalInternal(adjFaceIndices[j])).getNormalized();
PxU32 triCell = adjFaceIndices[j] >> 1;
PxU32 triRow = triCell/hf.getNbColumnsFast();
PxU32 triCol = triCell%hf.getNbColumnsFast();
PxVec3 tv0, tv1, tv2, tvc;
hf.getTriangleVertices(adjFaceIndices[j], triRow, triCol, tv0, tv1, tv2);
tvc = hfUtil.hf2shapep((tv0+tv1+tv2)/3.0f); // compute adjacent triangle center
PxVec3 perp = adjNormal.cross(direction).getNormalized(); // adj face normal cross edge dir
if (perp.dot(tvc-origin) < 0.0f) // make sure perp is pointing toward the center of the triangle
perp = -perp;
// perp is now a vector sticking out of the edge of the triangle (also the test edge) pointing toward the center
// perpendicular to the normal (in triangle plane)
if (perp.dot(localN) > 0.0f) // if the normal is in perp halfspace, clamp it to Voronoi region
{
n = transform1.rotate(adjNormal);
break;
}
}
} else if(featureType == HFU::eVERTEX)
{
// AP: these contacts are rare so hopefully it's ok
const PxU32 bufferCount = contactBuffer.count;
const PxU32 vertIndex = HFU::getFeatureIndex(closestFeatures[pi]);
EdgeData adjEdges[8];
const PxU32 row = vertIndex / nbColumns;
const PxU32 col = vertIndex % nbColumns;
const PxU32 numAdjEdges = ::getVertexEdgeIndices(hf, vertIndex, row, col, adjEdges);
for (PxU32 iPrevEdgeContact = numFaceContacts; iPrevEdgeContact < bufferCount; iPrevEdgeContact++)
{
if (contactBuffer.contacts[iPrevEdgeContact].forInternalUse != HFU::eEDGE)
continue; // skip non-edge contacts (can be other vertex contacts)
for (PxU32 iAdjEdge = 0; iAdjEdge < numAdjEdges; iAdjEdge++)
// does adjacent edge index for this vertex match a previously encountered edge index?
if (adjEdges[iAdjEdge].edgeIndex == contactBuffer.contacts[iPrevEdgeContact].internalFaceIndex1)
{
// if so, clamp the normal for this vertex to that edge's normal
n = contactBuffer.contacts[iPrevEdgeContact].normal;
dMag = PxSqrt(dMagSq);
break;
}
}
}
if (dMag == -1.0f)
n = hfUtil.computePointNormal(hfGeom.heightFieldFlags, d, transform1,
dMagSq, closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
PxVec3 p = transform0.p - n * radius;
#if DEBUG_RENDER_HFCONTACTS
printf("n=%.5f %.5f %.5f; ", n.x, n.y, n.z);
if (n.y < 0.8f)
int a = 1;
PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
<< p << (p + n * 10.0f);
#endif
// temporarily use the internalFaceIndex0 slot in the contact buffer for featureType
contactBuffer.contact(
p, n, dMag - radius, PxU16(featureType), HFU::getFeatureIndex(closestFeatures[pi]));
}
}
}
}
void UBodySetup::AddConvexElemsToRigidActor(PxRigidActor* PDestActor, const FTransform& RelativeTM, const FVector& Scale3D, const FVector& Scale3DAbs, TArray<PxShape*>* NewShapes) const
{
float ContactOffsetFactor, MaxContactOffset;
GetContactOffsetParams(ContactOffsetFactor, MaxContactOffset);
PxMaterial* PDefaultMat = GetDefaultPhysMaterial();
for (int32 i = 0; i < AggGeom.ConvexElems.Num(); i++)
{
const FKConvexElem& ConvexElem = AggGeom.ConvexElems[i];
if (ConvexElem.ConvexMesh)
{
PxTransform PLocalPose;
bool bUseNegX = CalcMeshNegScaleCompensation(Scale3D, PLocalPose);
PxConvexMeshGeometry PConvexGeom;
PConvexGeom.convexMesh = bUseNegX ? ConvexElem.ConvexMeshNegX : ConvexElem.ConvexMesh;
PConvexGeom.scale.scale = U2PVector(Scale3DAbs * ConvexElem.GetTransform().GetScale3D().GetAbs());
FTransform ConvexTransform = ConvexElem.GetTransform();
if (ConvexTransform.GetScale3D().X < 0 || ConvexTransform.GetScale3D().Y < 0 || ConvexTransform.GetScale3D().Z < 0)
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] has negative scale. Not currently supported"), *GetPathNameSafe(GetOuter()), i);
}
if (ConvexTransform.IsValid())
{
PxTransform PElementTransform = U2PTransform(ConvexTransform * RelativeTM);
PLocalPose.q *= PElementTransform.q;
PLocalPose.p = PElementTransform.p;
PLocalPose.p.x *= Scale3D.X;
PLocalPose.p.y *= Scale3D.Y;
PLocalPose.p.z *= Scale3D.Z;
if (PConvexGeom.isValid())
{
PxVec3 PBoundsExtents = PConvexGeom.convexMesh->getLocalBounds().getExtents();
ensure(PLocalPose.isValid());
PxShape* NewShape = PDestActor->createShape(PConvexGeom, *PDefaultMat, PLocalPose);
if (NewShapes)
{
NewShapes->Add(NewShape);
}
const float ContactOffset = FMath::Min(MaxContactOffset, ContactOffsetFactor * PBoundsExtents.minElement());
NewShape->setContactOffset(ContactOffset);
}
else
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] invalid"), *GetPathNameSafe(GetOuter()), i);
}
}
else
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] has invalid transform"), *GetPathNameSafe(GetOuter()), i);
}
}
else
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: ConvexElem is missing ConvexMesh (%d: %s)"), i, *GetPathName());
}
}
}
void setupFinalizeExtSolverContacts(
const ContactPoint* buffer,
const CorrelationBuffer& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
PxU8* workspace,
const SolverExtBody& b0,
const SolverExtBody& b1,
const PxReal invDtF32,
PxReal bounceThresholdF32,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1,
const PxReal restDist,
PxU8* frictionDataPtr,
PxReal ccdMaxContactDist)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
/*const bool haveFriction = PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;*/
const FloatV ccdMaxSeparation = FLoad(ccdMaxContactDist);
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];
const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1));
const PxReal d0 = invMassScale0;
const PxReal d1 = invMassScale1;
const PxReal angD0 = invInertiaScale0;
const PxReal angD1 = invInertiaScale1;
Vec4V staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = V4Zero();
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetZ(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d0));
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetW(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d1));
const FloatV restDistance = FLoad(restDist);
PxU32 frictionPatchWritebackAddrIndex = 0;
PxU32 contactWritebackCount = 0;
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
const FloatV invDt = FLoad(invDtF32);
const FloatV p8 = FLoad(0.8f);
const FloatV bounceThreshold = FLoad(bounceThresholdF32);
const FloatV invDtp8 = FMul(invDt, p8);
PxU8 flags = 0;
for(PxU32 i=0;i<c.frictionPatchCount;i++)
{
PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const FrictionPatch& frictionPatch = c.frictionPatches[i];
PX_ASSERT(frictionPatch.anchorCount <= 2); //0==anchorCount is allowed if all the contacts in the manifold have a large offset.
const Gu::ContactPoint* contactBase0 = buffer + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal combinedRestitution = contactBase0->restitution;
const PxReal staticFriction = contactBase0->staticFriction;
const PxReal dynamicFriction = contactBase0->dynamicFriction;
const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetX(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(staticFriction));
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetY(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(dynamicFriction));
SolverContactHeader* PX_RESTRICT header = reinterpret_cast<SolverContactHeader*>(ptr);
ptr += sizeof(SolverContactHeader);
Ps::prefetchLine(ptr + 128);
Ps::prefetchLine(ptr + 256);
Ps::prefetchLine(ptr + 384);
const bool haveFriction = (disableStrongFriction == 0) ;//PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;
header->numNormalConstr = Ps::to8(contactCount);
header->numFrictionConstr = Ps::to8(haveFriction ? frictionPatch.anchorCount*2 : 0);
header->type = Ps::to8(DY_SC_TYPE_EXT_CONTACT);
header->flags = flags;
const FloatV restitution = FLoad(combinedRestitution);
header->staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W;
header->angDom0 = angD0;
header->angDom1 = angD1;
const PxU32 pointStride = sizeof(SolverContactPointExt);
const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
const Vec3V normal = V3LoadU(buffer[c.contactPatches[c.correlationListHeads[i]].start].normal);
header->normal = normal;
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
p += pointStride;
setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDt, invDtp8, restDistance, maxPenBias, restitution,
bounceThreshold, contact, *solverContact, ccdMaxSeparation);
}
ptr = p;
}
contactWritebackCount += contactCount;
PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
PxMemZero(forceBuffer, sizeof(PxF32) * contactCount);
ptr += sizeof(PxF32) * ((contactCount + 3) & (~3));
header->broken = 0;
if(haveFriction)
{
//const Vec3V normal = Vec3V_From_PxVec3(buffer.contacts[c.contactPatches[c.correlationListHeads[i]].start].normal);
PxVec3 normalS = buffer[c.contactPatches[c.correlationListHeads[i]].start].normal;
PxVec3 t0, t1;
computeFrictionTangents(b0.getLinVel() - b1.getLinVel(), normalS, t0, t1);
Vec3V vT0 = V3LoadU(t0);
Vec3V vT1 = V3LoadU(t1);
//We want to set the writeBack ptr to point to the broken flag of the friction patch.
//On spu we have a slight problem here because the friction patch array is
//in local store rather than in main memory. The good news is that the address of the friction
//patch array in main memory is stored in the work unit. These two addresses will be equal
//except on spu where one is local store memory and the other is the effective address in main memory.
//Using the value stored in the work unit guarantees that the main memory address is used on all platforms.
PxU8* PX_RESTRICT writeback = frictionDataPtr + frictionPatchWritebackAddrIndex*sizeof(FrictionPatch);
header->frictionBrokenWritebackByte = writeback;
for(PxU32 j = 0; j < frictionPatch.anchorCount; j++)
{
SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
ptr += frictionStride;
SolverContactFrictionExt* PX_RESTRICT f1 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
ptr += frictionStride;
PxVec3 ra = bodyFrame0.q.rotate(frictionPatch.body0Anchors[j]);
PxVec3 rb = bodyFrame1.q.rotate(frictionPatch.body1Anchors[j]);
PxVec3 error = (ra + bodyFrame0.p) - (rb + bodyFrame1.p);
{
const PxVec3 raXn = ra.cross(t0);
const PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1));
const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);
PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
PxF32 targetVel = buffer[index].targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel -= b0.projectVelocity(t0, raXn);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel += b1.projectVelocity(t0, rbXn);
f0->normalXYZ_appliedForceW = V4SetW(vT0, zero);
f0->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
f0->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t0.dot(error) * invDtF32));
f0->linDeltaVA = V3LoadA(deltaV0.linear);
f0->angDeltaVA = V3LoadA(deltaV0.angular);
f0->linDeltaVB = V3LoadA(deltaV1.linear);
f0->angDeltaVB = V3LoadA(deltaV1.angular);
f0->targetVel = targetVel;
}
{
const PxVec3 raXn = ra.cross(t1);
const PxVec3 rbXn = rb.cross(t1);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t1, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1));
const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);
PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
PxF32 targetVel = buffer[index].targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel -= b0.projectVelocity(t1, raXn);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel += b1.projectVelocity(t1, rbXn);
f1->normalXYZ_appliedForceW = V4SetW(vT1, zero);
f1->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
f1->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t1.dot(error) * invDtF32));
f1->linDeltaVA = V3LoadA(deltaV0.linear);
f1->angDeltaVA = V3LoadA(deltaV0.angular);
f1->linDeltaVB = V3LoadA(deltaV1.linear);
f1->angDeltaVB = V3LoadA(deltaV1.angular);
f1->targetVel = targetVel;
}
}
}
frictionPatchWritebackAddrIndex++;
}
}
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;
}
/**
@brief
@date 2013-12-03
*/
void CEvc::spawnNode( const int key )
{
PxSceneWriteLock scopedLock(*mScene);
PxVec3 pos = getCamera().getPos() + (getCamera().getViewDir()*10.f);
const PxVec3 vel = getCamera().getViewDir() * 20.f;
// find earth position
{
PxU32 width, height;
mApplication.getPlatform()->getWindowSize(width, height);
PxVec3 rayOrig, rayDir, pickOrig;
mPicking->computeCameraRay(rayOrig, rayDir, pickOrig, width/2, height/2);
// raycast rigid bodies in scene
PxRaycastHit hit;
hit.shape = NULL;
const PxU32 bufferSize = 256; // [in] size of 'hitBuffer'
PxRaycastHit hitBuffer[bufferSize]; // [out] User provided buffer for results
PxRaycastBuffer buf(hitBuffer, bufferSize); // [out] Blocking and touching hits will be stored here
mScene->raycast(rayOrig, rayDir, PX_MAX_F32, buf);
for (PxU32 i = 0; i < buf.nbTouches; i++)
{
BOOST_FOREACH (auto planet, m_Planet)
{
if (buf.touches[ i].actor == planet)
{
hit = buf.touches[ i];
break;
}
}
if (hit.shape)
break;
}
if (!hit.shape)
return;
pos = hit.position;
}
evc::CCreature *creature = NULL;
bool IsCreature = true;
switch (key)
{
case SPAWN_DEBUG_OBJECT:
case SPAWN_DEBUG_OBJECT2:
case SPAWN_DEBUG_OBJECT3:
case SPAWN_DEBUG_OBJECT4:
case SPAWN_DEBUG_OBJECT5:
case SPAWN_DEBUG_OBJECT6:
{
string fileNames[] = {
string("genotype.txt"),
string("genotype_box.txt"),
string("genotype_herb.txt"),
string("genotype_tree.txt"),
string("genotype_flower.txt"),
string("genotype_bug.txt")
};
const int idx = key - SPAWN_DEBUG_OBJECT;
if ((sizeof(fileNames)/sizeof(string)) <= idx)
return;
if (SPAWN_DEBUG_OBJECT2 == key)
{
pos = getCamera().getPos() + (getCamera().getViewDir()*10.f);
}
else if (SPAWN_DEBUG_OBJECT6 == key)
{
PxVec3 dir = pos;
dir.normalize();
//pos += (dir * 5.f);
pos += PxVec3(0,5,0);
}
creature = new evc::CCreature(*this);
creature->SetMaxGrowCount(g_pDbgConfig->generationRecursiveCount);
m_Creatures.push_back( creature );
if (SPAWN_DEBUG_OBJECT6 == key)
{
//pnode->GenerateImmediate(fileNames[ idx], pos, NULL, g_pDbgConfig->generationRecursiveCount, g_pDbgConfig->displaySkinning);
creature->GenerateProgressive(fileNames[ idx], pos, NULL, g_pDbgConfig->displaySkinning);
}
else
{
creature->GenerateProgressive(fileNames[ idx], pos, ((SPAWN_DEBUG_OBJECT2 == key)? &vel : NULL), g_pDbgConfig->displaySkinning);
}
m_genotypeController->ControllerSceneInit();
m_genotypeController->SetControlCreature(creature);
}
break;
case SPAWN_DEBUG_OBJECT9:
{
creature = new evc::CCreature(*this);
creature->GenerateImmediate("genotype_flower.txt", pos, NULL, g_pDbgConfig->generationRecursiveCount, g_pDbgConfig->displaySkinning);
m_Creatures.push_back( creature );
//PxVec3 initialPos(-50,10,0);
//for (int i=0; i < 30; ++i)
//{
// pnode = new evc::CCreature(*this);
// pnode->GenerateByGenotype("genotype.txt", initialPos+PxVec3((i%10)*m_Gap, 0, (i/10)*m_Gap));
// m_Creatures.push_back( pnode );
//}
//IsCreature = false;
}
break;
//case SPAWN_DEBUG_OBJECT3: pnode->GenerateHuman3(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT4: pnode->GenerateHuman4(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT5: pnode->GenerateHuman5(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT6: pnode->GenerateHuman6(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT7: pnode->GenerateHuman7(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT8: pnode->GenerateHuman8(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT9: pnode->GenerateHuman9(g_pDbgConfig->applyJoint); break;
//case SPAWN_DEBUG_OBJECT0: pnode->GenerateByGenotype("genotype.txt"); break;
}
RET(!creature);
}
static PX_FORCE_INLINE int testSeparationAxes( const PxTriangle& tri, const PxVec3& extents,
const PxVec3& normal, const PxVec3& dir, const PxVec3& oneOverDir, float tmax, float& tcoll)
{
#ifdef _XBOX
float bValidMTD = 1.0f;
#else
bool bValidMTD = true;
#endif
tcoll = tmax;
float tfirst = -FLT_MAX;
float tlast = FLT_MAX;
// Triangle normal
#ifdef _XBOX
if(testAxis(tri, extents, dir, normal, bValidMTD, tfirst, tlast)==0.0f)
#else
if(!testAxis(tri, extents, dir, normal, bValidMTD, tfirst, tlast))
#endif
return 0;
// Box normals
#ifdef _XBOX
if(testAxisXYZ(tri, extents, dir, oneOverDir.x, bValidMTD, tfirst, tlast, 0)==0.0f)
return 0;
if(testAxisXYZ(tri, extents, dir, oneOverDir.y, bValidMTD, tfirst, tlast, 1)==0.0f)
return 0;
if(testAxisXYZ(tri, extents, dir, oneOverDir.z, bValidMTD, tfirst, tlast, 2)==0.0f)
return 0;
#else
if(!testAxisXYZ(tri, extents, dir, oneOverDir.x, bValidMTD, tfirst, tlast, 0))
return 0;
if(!testAxisXYZ(tri, extents, dir, oneOverDir.y, bValidMTD, tfirst, tlast, 1))
return 0;
if(!testAxisXYZ(tri, extents, dir, oneOverDir.z, bValidMTD, tfirst, tlast, 2))
return 0;
#endif
// Edges
for(PxU32 i=0; i<3; i++)
{
int ip1 = int(i+1);
if(i>=2) ip1 = 0;
const PxVec3 TriEdge = tri.verts[ip1] - tri.verts[i];
#ifdef _XBOX
{
const PxVec3 Sep = Ps::cross100(TriEdge);
if((Sep.dot(Sep))>=1.0E-6f && testAxis(tri, extents, dir, Sep, bValidMTD, tfirst, tlast)==0.0f)
return 0;
}
{
const PxVec3 Sep = Ps::cross010(TriEdge);
if((Sep.dot(Sep))>=1.0E-6f && testAxis(tri, extents, dir, Sep, bValidMTD, tfirst, tlast)==0.0f)
return 0;
}
{
const PxVec3 Sep = Ps::cross001(TriEdge);
if((Sep.dot(Sep))>=1.0E-6f && testAxis(tri, extents, dir, Sep, bValidMTD, tfirst, tlast)==0.0f)
return 0;
}
#else
{
const PxVec3 Sep = Ps::cross100(TriEdge);
if((Sep.dot(Sep))>=1.0E-6f && !testAxis(tri, extents, dir, Sep, bValidMTD, tfirst, tlast))
return 0;
}
{
const PxVec3 Sep = Ps::cross010(TriEdge);
if((Sep.dot(Sep))>=1.0E-6f && !testAxis(tri, extents, dir, Sep, bValidMTD, tfirst, tlast))
return 0;
}
{
const PxVec3 Sep = Ps::cross001(TriEdge);
if((Sep.dot(Sep))>=1.0E-6f && !testAxis(tri, extents, dir, Sep, bValidMTD, tfirst, tlast))
return 0;
}
#endif
}
if(tfirst > tmax || tlast < 0.0f) return 0;
if(tfirst <= 0.0f)
{
#ifdef _XBOX
if(bValidMTD==0.0f) return 0;
#else
if(!bValidMTD) return 0;
#endif
tcoll = 0.0f;
}
else tcoll = tfirst;
return 1;
}
bool Gu::sweepBoxSphere(const Box& box, PxReal sphereRadius, const PxVec3& spherePos, const PxVec3& dir, PxReal length, PxReal& min_dist, PxVec3& normal, PxHitFlags hitFlags)
{
if(!(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP))
{
// PT: test if shapes initially overlap
if(intersectSphereBox(Sphere(spherePos, sphereRadius), box))
{
// Overlap
min_dist = 0.0f;
normal = -dir;
return true;
}
}
PxVec3 boxPts[8];
box.computeBoxPoints(boxPts);
const PxU8* PX_RESTRICT edges = getBoxEdges();
PxReal MinDist = length;
bool Status = false;
for(PxU32 i=0; i<12; i++)
{
const PxU8 e0 = *edges++;
const PxU8 e1 = *edges++;
const Capsule capsule(boxPts[e0], boxPts[e1], sphereRadius);
PxReal t;
if(intersectRayCapsule(spherePos, dir, capsule, t))
{
if(t>=0.0f && t<=MinDist)
{
MinDist = t;
const PxVec3 ip = spherePos + t*dir;
distancePointSegmentSquared(capsule, ip, &t);
PxVec3 ip2;
capsule.computePoint(ip2, t);
normal = (ip2 - ip);
normal.normalize();
Status = true;
}
}
}
PxVec3 localPt;
{
Matrix34 M2;
buildMatrixFromBox(M2, box);
localPt = M2.rotateTranspose(spherePos - M2.p);
}
const PxVec3* boxNormals = gNearPlaneNormal;
const PxVec3 localDir = box.rotateInv(dir);
// PT: when the box exactly touches the sphere, the test for initial overlap can fail on some platforms.
// In this case we reach the sweep code below, which may return a slightly negative time of impact (it should be 0.0
// but it ends up a bit negative because of limited FPU accuracy). The epsilon ensures that we correctly detect a hit
// in this case.
const PxReal epsilon = -1e-5f;
PxReal tnear, tfar;
PxVec3 extents = box.extents;
extents.x += sphereRadius;
int plane = intersectRayAABB(-extents, extents, localPt, localDir, tnear, tfar);
if(plane!=-1 && tnear>=epsilon && tnear <= MinDist)
{
MinDist = PxMax(tnear, 0.0f);
normal = box.rotate(boxNormals[plane]);
Status = true;
}
extents = box.extents;
extents.y += sphereRadius;
plane = intersectRayAABB(-extents, extents, localPt, localDir, tnear, tfar);
if(plane!=-1 && tnear>=epsilon && tnear <= MinDist)
{
MinDist = PxMax(tnear, 0.0f);
normal = box.rotate(boxNormals[plane]);
Status = true;
}
extents = box.extents;
extents.z += sphereRadius;
plane = intersectRayAABB(-extents, extents, localPt, localDir, tnear, tfar);
if(plane!=-1 && tnear>=epsilon && tnear <= MinDist)
{
MinDist = PxMax(tnear, 0.0f);
normal = box.rotate(boxNormals[plane]);
Status = true;
}
min_dist = MinDist;
return Status;
}
// 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;
}
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;
}
}
}
//! Compute the smallest distance from the (infinite) line to the box.
static PxReal distanceLineBoxSquared(const PxVec3& lineOrigin, const PxVec3& lineDirection,
const PxVec3& boxOrigin, const PxVec3& boxExtent, const PxMat33& boxBase,
PxReal* lineParam,
PxVec3* boxParam)
{
const PxVec3& axis0 = boxBase.column0;
const PxVec3& axis1 = boxBase.column1;
const PxVec3& axis2 = boxBase.column2;
// compute coordinates of line in box coordinate system
const PxVec3 diff = lineOrigin - boxOrigin;
PxVec3 pnt(diff.dot(axis0), diff.dot(axis1), diff.dot(axis2));
PxVec3 dir(lineDirection.dot(axis0), lineDirection.dot(axis1), lineDirection.dot(axis2));
// Apply reflections so that direction vector has nonnegative components.
bool reflect[3];
for(unsigned int i=0;i<3;i++)
{
if(dir[i]<0.0f)
{
pnt[i] = -pnt[i];
dir[i] = -dir[i];
reflect[i] = true;
}
else
{
reflect[i] = false;
}
}
PxReal sqrDistance = 0.0f;
if(dir.x>0.0f)
{
if(dir.y>0.0f)
{
if(dir.z>0.0f) caseNoZeros(pnt, dir, boxExtent, lineParam, sqrDistance); // (+,+,+)
else case0(0, 1, 2, pnt, dir, boxExtent, lineParam, sqrDistance); // (+,+,0)
}
else
{
if(dir.z>0.0f) case0(0, 2, 1, pnt, dir, boxExtent, lineParam, sqrDistance); // (+,0,+)
else case00(0, 1, 2, pnt, dir, boxExtent, lineParam, sqrDistance); // (+,0,0)
}
}
else
{
if(dir.y>0.0f)
{
if(dir.z>0.0f) case0(1, 2, 0, pnt, dir, boxExtent, lineParam, sqrDistance); // (0,+,+)
else case00(1, 0, 2, pnt, dir, boxExtent, lineParam, sqrDistance); // (0,+,0)
}
else
{
if(dir.z>0.0f) case00(2, 0, 1, pnt, dir, boxExtent, lineParam, sqrDistance); // (0,0,+)
else
{
case000(pnt, boxExtent, sqrDistance); // (0,0,0)
if(lineParam)
*lineParam = 0.0f;
}
}
}
if(boxParam)
{
// undo reflections
for(unsigned int i=0;i<3;i++)
{
if(reflect[i])
pnt[i] = -pnt[i];
}
*boxParam = pnt;
}
return sqrDistance;
}
///////////////////////////////////////////////////////////////////////////////
// compute intersection of a ray from a source vertex in direction toward parent
int PxClothGeodesicTetherCookerImpl::computeVertexIntersection(PxU32 parent, PxU32 src, PathIntersection &path)
{
if (src == parent)
{
path = PathIntersection(true, src, 0.0);
return 0;
}
float maxdot = -1.0f;
int closestVert = -1;
// gradient is toward the parent vertex
PxVec3 g = (mVertices[parent] - mVertices[src]).getNormalized();
// for every triangle incident on this vertex, we intersect against opposite edge of the triangle
PxU32 sfirst = mFirstVertTriAdj[src];
PxU32 slast = (src < ((PxU32)mNumParticles-1)) ? mFirstVertTriAdj[src+1] : (PxU32)mVertTriAdjs.size();
for (PxU32 adj = sfirst; adj < slast; adj++)
{
PxU32 tid = mVertTriAdjs[adj];
PxU32 i0 = mIndices[tid*3];
PxU32 i1 = mIndices[tid*3+1];
PxU32 i2 = mIndices[tid*3+2];
int eid = 0;
if (i0 == src) eid = 1;
else if (i1 == src) eid = 2;
else if (i2 == src) eid = 0;
else continue; // error
// reshuffle so that src is located at i2
i0 = mIndices[tid*3 + eid];
i1 = mIndices[tid*3 + (eid+1)%3];
i2 = src;
PxVec3 p0 = mVertices[i0];
PxVec3 p1 = mVertices[i1];
PxVec3 p2 = mVertices[i2];
// check if we hit source immediately from this triangle
if (i0 == parent)
{
path = PathIntersection(true, parent, (p0 - p2).magnitude());
return 1;
}
if (i1 == parent)
{
path = PathIntersection(true, parent, (p1 - p2).magnitude());
return 1;
}
// ray direction is the gradient projected on the plane of this triangle
PxVec3 n = ((p0 - p2).cross(p1 - p2)).getNormalized();
PxVec3 d = (g - g.dot(n) * n).getNormalized();
// find intersection of ray (p2, d) against the edge (p0,p1)
float s, t;
bool result = intersectRayEdge(p2, d, p0, p1, s, t);
if (result == false)
continue;
// t should be positive, otherwise we just hit the triangle in opposite direction, so ignore
const float eps = 1e-5;
if (t > -eps)
{
PxVec3 ip; // intersection point
if (( s > -eps ) && (s < (1.0f + eps)))
{
// if intersection point is too close to each vertex, we record a vertex intersection
if ( ( s < eps) || (s > (1.0f-eps)))
{
path.vertOrTriangle = true;
path.index = (s < eps) ? i0 : i1;
path.distance = (p2 - mVertices[path.index]).magnitude();
}
else // found an edge instersection
{
ip = p0 + s * (p1 - p0);
path = PathIntersection(false, tid*3 + eid, (p2 - ip).magnitude(), s);
}
return 1;
}
}
// for fall back (see below)
PxVec3 d0 = (p0 - p2).getNormalized();
PxVec3 d1 = (p1 - p2).getNormalized();
float d0dotg = d0.dot(d);
float d1dotg = d1.dot(d);
if (d0dotg > maxdot)
{
closestVert = (int)i0;
maxdot = d0dotg;
}
if (d1dotg > maxdot)
{
closestVert = (int)i1;
maxdot = d1dotg;
}
} // end for (PxU32 adj = sfirst...
// Fall back to use greedy (Dijkstra-like) path selection.
// This happens as triangles are curved and we may not find intersection on any triangle.
// In this case, we choose a vertex closest to the gradient direction.
if (closestVert > 0)
{
path = PathIntersection(true, (PxU32)closestVert, (mVertices[src] - mVertices[(PxU32)closestVert]).magnitude());
return 1;
}
// Error, (possibly dangling vertex)
return -1;
}
// Compute depenetration vector and distance if possible with a slightly larger geometry
static bool ComputeInflatedMTD(const float MtdInflation, const PxLocationHit& PHit, FHitResult& OutResult, const PxTransform& QueryTM, const PxGeometry& Geom, const PxTransform& PShapeWorldPose)
{
switch(Geom.getType())
{
case PxGeometryType::eCAPSULE:
{
const PxCapsuleGeometry* InCapsule = static_cast<const PxCapsuleGeometry*>(&Geom);
PxCapsuleGeometry InflatedCapsule(InCapsule->radius + MtdInflation, InCapsule->halfHeight); // don't inflate halfHeight, radius is added all around.
return ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, QueryTM, InflatedCapsule, PShapeWorldPose);
}
case PxGeometryType::eBOX:
{
const PxBoxGeometry* InBox = static_cast<const PxBoxGeometry*>(&Geom);
PxBoxGeometry InflatedBox(InBox->halfExtents + PxVec3(MtdInflation));
return ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, QueryTM, InflatedBox, PShapeWorldPose);
}
case PxGeometryType::eSPHERE:
{
const PxSphereGeometry* InSphere = static_cast<const PxSphereGeometry*>(&Geom);
PxSphereGeometry InflatedSphere(InSphere->radius + MtdInflation);
return ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, QueryTM, InflatedSphere, PShapeWorldPose);
}
case PxGeometryType::eCONVEXMESH:
{
// We can't exactly inflate the mesh (not easily), so try jittering it a bit to get an MTD result.
PxVec3 TraceDir = U2PVector(OutResult.TraceEnd - OutResult.TraceStart);
TraceDir.normalizeSafe();
// Try forward (in trace direction)
PxTransform JitteredTM = PxTransform(QueryTM.p + (TraceDir * MtdInflation), QueryTM.q);
if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
{
return true;
}
// Try backward (opposite trace direction)
JitteredTM = PxTransform(QueryTM.p - (TraceDir * MtdInflation), QueryTM.q);
if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
{
return true;
}
// Try axial directions.
// Start with -Z because this is the most common case (objects on the floor).
for (int32 i=2; i >= 0; i--)
{
PxVec3 Jitter(0.f);
Jitter[i] = MtdInflation;
JitteredTM = PxTransform(QueryTM.p - Jitter, QueryTM.q);
if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
{
return true;
}
JitteredTM = PxTransform(QueryTM.p + Jitter, QueryTM.q);
if (ComputeInflatedMTD_Internal(MtdInflation, PHit, OutResult, JitteredTM, Geom, PShapeWorldPose))
{
return true;
}
}
return false;
}
default:
{
return false;
}
}
}
///////////////////////////////////////////////////////////////////////////////
// compute intersection of a ray from a source vertex in direction toward parent
int PxClothGeodesicTetherCookerImpl::computeEdgeIntersection(PxU32 parent, PxU32 edge, float in_s, PathIntersection &path)
{
int tid = (int)edge / 3;
int eid = (int)edge % 3;
PxU32 e0 = mIndices[PxU32(tid*3 + eid)];
PxU32 e1 = mIndices[PxU32(tid*3 + (eid+1)%3)];
PxVec3 v0 = mVertices[e0];
PxVec3 v1 = mVertices[e1];
PxVec3 v = v0 + in_s * (v1 - v0);
PxVec3 g = mVertices[parent] - v;
PxU32 triNbr = mTriNeighbors[edge];
if (triNbr == PxU32(-1)) // boundary edge
{
float dir = g.dot(v1-v0);
PxU32 vid = (dir > 0) ? e1 : e0;
path = PathIntersection(true, vid, (mVertices[vid] - v).magnitude());
return 1;
}
PxU32 i0 = mIndices[triNbr*3];
PxU32 i1 = mIndices[triNbr*3+1];
PxU32 i2 = mIndices[triNbr*3+2];
// vertex is sorted s.t i0,i1 contains the edge point
if ( checkEdge((int)i0, (int)i1, (int)e0, (int)e1)) {
eid = 0;
}
else if ( checkEdge((int)i1, (int)i2, (int)e0, (int)e1)) {
eid = 1;
PxU32 tmp = i2;
i2 = i0;
i0 = i1;
i1 = tmp;
}
else if ( checkEdge((int)i2, (int)i0, (int)e0, (int)e1))
{
eid = 2;
PxU32 tmp = i0;
i0 = i2;
i2 = i1;
i1 = tmp;
}
// we hit the parent
if (i2 == parent)
{
path = PathIntersection(true, i2, (mVertices[i2] - v).magnitude());
return 1;
}
PxVec3 p0 = mVertices[i0];
PxVec3 p1 = mVertices[i1];
PxVec3 p2 = mVertices[i2];
// project gradient vector on the plane of the triangle
PxVec3 n = ((p0 - p2).cross(p1 - p2)).getNormalized();
g = (g - g.dot(n) * n).getNormalized();
float s = 0.0f, t = 0.0f;
const float eps = 1e-5;
PxVec3 ip;
// intersect against edge form p2 to p0
if (intersectRayEdge(v, g, p2, p0, s, t) && ( s >= -eps) && ( s <= (1.0f+eps) ) && (t > -eps))
{
if ( ( s < eps) || (s > (1.0f-eps)))
{
path.vertOrTriangle = true;
path.index = (s < eps) ? i2 : i0;
path.distance = (mVertices[path.index] - v).magnitude();
}
else
{
ip = p2 + s * (p0 - p2);
path = PathIntersection(false, triNbr*3 + (eid + 2) % 3, (ip - v).magnitude(), s);
}
return 1;
}
// intersect against edge form p1 to p2
if (intersectRayEdge(v, g, p1, p2, s, t) && ( s >= -eps) && ( s <= (1.0f+eps) ) && (t > -eps))
{
if ( ( s < eps) || (s > (1.0f-eps)))
{
path.vertOrTriangle = true;
path.index = (s < eps) ? i1 : i2;
path.distance = (mVertices[path.index] - v).magnitude();
}
else
{
ip = p1 + s * (p2 - p1);
path = PathIntersection(false, triNbr*3 + (eid + 1) % 3, (ip - v).magnitude(), s);
}
return 1;
}
// fallback to pick closer vertex when no edges intersect
float dir = g.dot(v1-v0);
path.vertOrTriangle = true;
path.index = (dir > 0) ? e1 : e0;
path.distance = (mVertices[path.index] - v).magnitude();
return 1;
}
bool setupFinalizeExtSolverConstraintsCoulomb(PxcNpWorkUnit& n,
const ContactBuffer& buffer,
const PxcCorrelationBufferCoulomb& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
bool /*perPointFriction*/,
PxU8* workspace,
PxReal invDt,
PxReal bounceThreshold,
PxsSolverExtBody& b0,
PxsSolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b0.mLinkIndex];
const FloatV maxPen = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(n.restDistance);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
bool useExtContacts = (n.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY0|PxcNpWorkUnitFlag::eARTICULATION_BODY1))!=0;
const PxU32 frictionPatchCount = c.frictionPatchCount;
const bool staticBody = ((n.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) == 0);
const PxU32 pointStride = useExtContacts ? sizeof(PxcSolverContactExt) : sizeof(PxcSolverContact);
const PxU32 frictionStride = useExtContacts ? sizeof(PxcSolverFrictionExt) : sizeof(PxcSolverFriction);
const PxU8 pointHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_CONTACT : (staticBody ? PXS_SC_TYPE_STATIC_CONTACT : PXS_SC_TYPE_RB_CONTACT));
const PxU8 frictionHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_FRICTION : (staticBody ? PXS_SC_TYPE_STATIC_FRICTION : PXS_SC_TYPE_FRICTION));
PxReal d0 = n.dominance0 * invMassScale0;
PxReal d1 = n.dominance1 * invMassScale1;
PxReal angD0 = n.dominance0 * invInertiaScale0;
PxReal angD1 = n.dominance1 * invInertiaScale1;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxcFrictionPatchCoulomb& frictionPatch = c.frictionPatches[i];
const Vec3V normalV = Ps::aos::V3LoadU(frictionPatch.normal);
const PxVec3 normal = frictionPatch.normal;
const PxReal combinedRestitution = contactBase0->restitution;
PxcSolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr);
ptr += sizeof(PxcSolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
header->numNormalConstr = (PxU8)contactCount;
header->type = pointHeaderType;
header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
PxcSolverContactExt* PX_RESTRICT solverContact = reinterpret_cast<PxcSolverContactExt*>(p);
p += pointStride;
const FloatV separation = FLoad(contact.separation);
PxVec3 ra = contact.point - bodyFrame0.p;
PxVec3 rb = contact.point - bodyFrame1.p;
Vec3V targetVel = V3LoadU(contact.targetVel);
const FloatV maxImpulse = FLoad(contact.maxImpulse);
solverContact->scaledBiasX_targetVelocityY_maxImpulseZ = V3Merge(FMax(maxPen, FSub(separation, restDistance)), V3Dot(normalV,targetVel), maxImpulse);
//TODO - should we do cross only in vector land and then store. Could cause a LHS but probably no worse than
//what we already have (probably has a LHS from converting from vector to scalar above)
const PxVec3 raXn = ra.cross(normal);
const PxVec3 rbXn = rb.cross(normal);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(normal, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-normal, -rbXn), deltaV1, d1, angD1);
const PxReal vrel = b0.projectVelocity(normal, raXn)
- b1.projectVelocity(normal, rbXn);
solverContact->raXnXYZ_appliedForceW = V4SetW(Vec4V_From_Vec3V(V3LoadU(raXn)), zero);
solverContact->rbXnXYZ_velMultiplierW = V4SetW(Vec4V_From_Vec3V(V3LoadU(rbXn)), zero);
completeContactPoint(*solverContact, unitResponse, vrel, invDt, header->restitution, bounceThreshold);
solverContact->setDeltaVA(deltaV0.linear, deltaV0.angular);
solverContact->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
PxcSolverContactCoulombHeader* header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(PxcSolverContactCoulombHeader) + header->numNormalConstr * pointStride;
PxVec3 normal = c.frictionPatches[i].normal;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal staticFriction = contactBase0->staticFriction;
const PxU32 disableStrongFriction = contactBase0->internalFaceIndex1 & PxMaterialFlag::eDISABLE_FRICTION;
const bool haveFriction = (disableStrongFriction == 0);
PxcSolverFrictionHeader* frictionHeader = (PxcSolverFrictionHeader*)ptr;
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatches[i].numConstraints : 0);
ptr += sizeof(PxcSolverFrictionHeader);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->setDominance0(n.dominance0);
frictionHeader->setDominance1(n.dominance1);
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const PxU32 contactId = totalPatchContactCount + j;
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
PxcSolverFrictionExt* PX_RESTRICT f0 = reinterpret_cast<PxcSolverFrictionExt*>(p);
p += frictionStride;
f0->contactIndex = contactId;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(t0, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-t0, -rbXn), deltaV1, d1, angD1);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(raXn);
f0->setRbXn(rbXn);
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->setDeltaVA(deltaV0.linear, deltaV0.angular);
f0->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
void Gu::TriangleMesh::debugVisualize(
Cm::RenderOutput& out, const PxTransform& pose, const PxMeshScale& scaling, const PxBounds3& cullbox,
const PxU64 mask, const PxReal fscale, const PxU32 numMaterials) const
{
PX_UNUSED(numMaterials);
//bool cscale = !!(mask & ((PxU64)1 << PxVisualizationParameter::eCULL_BOX));
const PxU64 cullBoxMask = PxU64(1) << PxVisualizationParameter::eCULL_BOX;
bool cscale = ((mask & cullBoxMask) == cullBoxMask);
const PxMat44 midt(PxIdentity);
const Cm::Matrix34 absPose(PxMat33(pose.q) * scaling.toMat33(), pose.p);
PxU32 nbTriangles = getNbTrianglesFast();
const PxU32 nbVertices = getNbVerticesFast();
const PxVec3* vertices = getVerticesFast();
const void* indices = getTrianglesFast();
const PxDebugColor::Enum colors[] =
{
PxDebugColor::eARGB_BLACK,
PxDebugColor::eARGB_RED,
PxDebugColor::eARGB_GREEN,
PxDebugColor::eARGB_BLUE,
PxDebugColor::eARGB_YELLOW,
PxDebugColor::eARGB_MAGENTA,
PxDebugColor::eARGB_CYAN,
PxDebugColor::eARGB_WHITE,
PxDebugColor::eARGB_GREY,
PxDebugColor::eARGB_DARKRED,
PxDebugColor::eARGB_DARKGREEN,
PxDebugColor::eARGB_DARKBLUE,
};
const PxU32 colorCount = sizeof(colors)/sizeof(PxDebugColor::Enum);
if(cscale)
{
const Gu::Box worldBox(
(cullbox.maximum + cullbox.minimum)*0.5f,
(cullbox.maximum - cullbox.minimum)*0.5f,
PxMat33(PxIdentity));
// PT: TODO: use the callback version here to avoid allocating this huge array
PxU32* results = reinterpret_cast<PxU32*>(PX_ALLOC_TEMP(sizeof(PxU32)*nbTriangles, "tmp triangle indices"));
LimitedResults limitedResults(results, nbTriangles, 0);
Midphase::intersectBoxVsMesh(worldBox, *this, pose, scaling, &limitedResults);
nbTriangles = limitedResults.mNbResults;
if (fscale)
{
const PxU32 fcolor = PxU32(PxDebugColor::eARGB_DARKRED);
for (PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results[i];
PxVec3 wp[3];
getTriangle(*this, index, wp, vertices, indices, absPose, has16BitIndices());
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_SHAPES))
{
const PxU32 scolor = PxU32(PxDebugColor::eARGB_MAGENTA);
out << midt << scolor; // PT: no need to output this for each segment!
PxDebugLine* segments = out.reserveSegments(nbTriangles*3);
for(PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results[i];
PxVec3 wp[3];
getTriangle(*this, index, wp, vertices, indices, absPose, has16BitIndices());
segments[0] = PxDebugLine(wp[0], wp[1], scolor);
segments[1] = PxDebugLine(wp[1], wp[2], scolor);
segments[2] = PxDebugLine(wp[2], wp[0], scolor);
segments+=3;
}
}
if ((mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_EDGES)) && mExtraTrigData)
visualizeActiveEdges(out, *this, nbTriangles, results, absPose, midt);
PX_FREE(results);
}
else
{
if (fscale)
{
const PxU32 fcolor = PxU32(PxDebugColor::eARGB_DARKRED);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices());
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_SHAPES))
{
PxU32 scolor = PxU32(PxDebugColor::eARGB_MAGENTA);
out << midt << scolor; // PT: no need to output this for each segment!
PxVec3* transformed = reinterpret_cast<PxVec3*>(PX_ALLOC(sizeof(PxVec3)*nbVertices, "PxVec3"));
for(PxU32 i=0;i<nbVertices;i++)
transformed[i] = absPose.transform(vertices[i]);
PxDebugLine* segments = out.reserveSegments(nbTriangles*3);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, transformed, indices, has16BitIndices());
const PxU32 localMaterialIndex = getTriangleMaterialIndex(i);
scolor = colors[localMaterialIndex % colorCount];
segments[0] = PxDebugLine(wp[0], wp[1], scolor);
segments[1] = PxDebugLine(wp[1], wp[2], scolor);
segments[2] = PxDebugLine(wp[2], wp[0], scolor);
segments+=3;
}
PX_FREE(transformed);
}
if ((mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_EDGES)) && mExtraTrigData)
visualizeActiveEdges(out, *this, nbTriangles, NULL, absPose, midt);
}
}
