PxBounds3 RTree::refitRecursive(PxU8* treeNodes8, PxU32 top, RTreePage* parentPage, PxU32 parentSubIndex, CallbackRefit& cb)
{
const PxReal eps = RTREE_INFLATION_EPSILON;
RTreePage* tn = (RTreePage*)(treeNodes8 + top);
PxBounds3 pageBound;
for (PxU32 i = 0; i < RTREE_PAGE_SIZE; i++)
{
if (tn->isEmpty(i))
continue;
PxU32 ptr = tn->ptrs[i];
Vec3V childMn, childMx;
PxBounds3 childBound;
if (ptr & 1)
{
// (ptr-1) clears the isLeaf bit (lowest bit)
cb.recomputeBounds(ptr-1, childMn, childMx); // compute the bound around triangles
V3StoreU(childMn, childBound.minimum);
V3StoreU(childMx, childBound.maximum);
// AP: doesn't seem worth vectorizing because of transposed layout
tn->minx[i] = childBound.minimum.x - eps; // update page bounds for this leaf
tn->miny[i] = childBound.minimum.y - eps;
tn->minz[i] = childBound.minimum.z - eps;
tn->maxx[i] = childBound.maximum.x + eps;
tn->maxy[i] = childBound.maximum.y + eps;
tn->maxz[i] = childBound.maximum.z + eps;
} else
childBound = refitRecursive(treeNodes8, ptr, tn, i, cb);
if (i == 0)
pageBound = childBound;
else
pageBound.include(childBound);
}
if (parentPage)
{
parentPage->minx[parentSubIndex] = pageBound.minimum.x - eps;
parentPage->miny[parentSubIndex] = pageBound.minimum.y - eps;
parentPage->minz[parentSubIndex] = pageBound.minimum.z - eps;
parentPage->maxx[parentSubIndex] = pageBound.maximum.x + eps;
parentPage->maxy[parentSubIndex] = pageBound.maximum.y + eps;
parentPage->maxz[parentSubIndex] = pageBound.maximum.z + eps;
}
return pageBound;
}
PxVec3 SolverExtBody::getAngVel() const
{
if(mLinkIndex == PxSolverConstraintDesc::NO_LINK)
return mBodyData->angularVelocity;
else
{
PxVec3 result;
V3StoreU(getVelocity(*mFsData)[mLinkIndex].angular, result);
return result;
}
}
void RTree::validateRecursive(PxU32 level, RTreeNodeQ parentBounds, RTreePage* page, CallbackRefit* cbLeaf)
{
PX_UNUSED(parentBounds);
static PxU32 validateCounter = 0; // this is to suppress a warning that recursive call has no side effects
validateCounter++;
RTreeNodeQ n;
PxU32 pageNodeCount = page->nodeCount();
for (PxU32 j = 0; j < pageNodeCount; j++)
{
page->getNode(j, n);
if (page->isEmpty(j))
continue;
PX_ASSERT(n.minx >= parentBounds.minx); PX_ASSERT(n.miny >= parentBounds.miny); PX_ASSERT(n.minz >= parentBounds.minz);
PX_ASSERT(n.maxx <= parentBounds.maxx); PX_ASSERT(n.maxy <= parentBounds.maxy); PX_ASSERT(n.maxz <= parentBounds.maxz);
if (!n.isLeaf())
{
PX_ASSERT((n.ptr&1) == 0);
RTreePage* childPage = (RTreePage*)(size_t(CONVERT_PTR_TO_INT(reinterpret_cast<size_t>(get64BitBasePage()))) + n.ptr);
validateRecursive(level+1, n, childPage, cbLeaf);
} else if (cbLeaf)
{
Vec3V mnv, mxv;
cbLeaf->recomputeBounds(page->ptrs[j] & ~1, mnv, mxv);
PxVec3 mn3, mx3; V3StoreU(mnv, mn3); V3StoreU(mxv, mx3);
const PxBounds3 lb(mn3, mx3);
const PxVec3& mn = lb.minimum; const PxVec3& mx = lb.maximum; PX_UNUSED(mn); PX_UNUSED(mx);
PX_ASSERT(mn.x >= n.minx); PX_ASSERT(mn.y >= n.miny); PX_ASSERT(mn.z >= n.minz);
PX_ASSERT(mx.x <= n.maxx); PX_ASSERT(mx.y <= n.maxy); PX_ASSERT(mx.z <= n.maxz);
}
}
RTreeNodeQ recomputedBounds;
page->computeBounds(recomputedBounds);
PX_ASSERT((recomputedBounds.minx - parentBounds.minx)<=RTREE_INFLATION_EPSILON);
PX_ASSERT((recomputedBounds.miny - parentBounds.miny)<=RTREE_INFLATION_EPSILON);
PX_ASSERT((recomputedBounds.minz - parentBounds.minz)<=RTREE_INFLATION_EPSILON);
PX_ASSERT((recomputedBounds.maxx - parentBounds.maxx)<=RTREE_INFLATION_EPSILON);
PX_ASSERT((recomputedBounds.maxy - parentBounds.maxy)<=RTREE_INFLATION_EPSILON);
PX_ASSERT((recomputedBounds.maxz - parentBounds.maxz)<=RTREE_INFLATION_EPSILON);
}
void solveFriction_BStatic(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
PxcSolverBody& b0 = *desc.bodyA;
Vec3V linVel0 = V3LoadA(b0.linearVelocity);
Vec3V angVel0 = V3LoadA(b0.angularVelocity);
const PxU8* PX_RESTRICT currPtr = desc.constraint;
const PxU8* PX_RESTRICT last = currPtr + getConstraintLength(desc);
//hopefully pointer aliasing doesn't bite.
//PxVec3 l0, a0;
//PxVec3_From_Vec3V(linVel0, l0);
//PxVec3_From_Vec3V(angVel0, a0);
//PX_ASSERT(l0.isFinite());
//PX_ASSERT(a0.isFinite());
while(currPtr < last)
{
const PxcSolverFrictionHeader* PX_RESTRICT frictionHeader = (PxcSolverFrictionHeader*)currPtr;
const PxU32 numFrictionConstr = frictionHeader->numFrictionConstr;
currPtr +=sizeof(PxcSolverFrictionHeader);
PxF32* appliedImpulse = (PxF32*)currPtr;
currPtr +=frictionHeader->getAppliedForcePaddingSize();
PxcSolverFriction* PX_RESTRICT frictions = (PxcSolverFriction*)currPtr;
currPtr += numFrictionConstr * sizeof(PxcSolverFriction);
const FloatV staticFriction = frictionHeader->getStaticFriction();
for(PxU32 i=0;i<numFrictionConstr;i++)
{
PxcSolverFriction& f = frictions[i];
Ps::prefetchLine(&frictions[i+1]);
const Vec3V t0 = Vec3V_From_Vec4V(f.normalXYZ_appliedForceW);
const Vec3V raXt0 = Vec3V_From_Vec4V(f.raXnXYZ_velMultiplierW);
const FloatV appliedForce = V4GetW(f.normalXYZ_appliedForceW);
const FloatV velMultiplier = V4GetW(f.raXnXYZ_velMultiplierW);
const FloatV targetVel = V4GetW(f.rbXnXYZ_targetVelocityW);
//const FloatV normalImpulse = contacts[f.contactIndex].getAppliedForce();
const FloatV normalImpulse = FLoad(appliedImpulse[f.contactIndex]);
const FloatV maxFriction = FMul(staticFriction, normalImpulse);
const FloatV nMaxFriction = FNeg(maxFriction);
//Compute the normal velocity of the constraint.
const FloatV t0Vel1 = V3Dot(t0, linVel0);
const FloatV t0Vel2 = V3Dot(raXt0, angVel0);
//const FloatV unbiasedErr = FMul(targetVel, velMultiplier);
//const FloatV biasedErr = FMulAdd(targetVel, velMultiplier, nScaledBias);
const FloatV t0Vel = FAdd(t0Vel1, t0Vel2);
const Vec3V delAngVel0 = Vec3V_From_Vec4V(f.delAngVel0_InvMassADom);
const Vec3V delLinVel0 = V3Scale(t0, V4GetW(f.delAngVel0_InvMassADom));
// still lots to do here: using loop pipelining we can interweave this code with the
// above - the code here has a lot of stalls that we would thereby eliminate
//FloatV deltaF = FSub(scaledBias, FMul(t0Vel, velMultiplier));//FNeg(FMul(t0Vel, velMultiplier));
//FloatV deltaF = FMul(t0Vel, velMultiplier);
//FloatV newForce = FMulAdd(t0Vel, velMultiplier, appliedForce);
const FloatV tmp = FNegMulSub(targetVel,velMultiplier,appliedForce);
FloatV newForce = FMulAdd(t0Vel, velMultiplier, tmp);
newForce = FClamp(newForce, nMaxFriction, maxFriction);
const FloatV deltaF = FSub(newForce, appliedForce);
linVel0 = V3ScaleAdd(delLinVel0, deltaF, linVel0);
angVel0 = V3ScaleAdd(delAngVel0, deltaF, angVel0);
f.setAppliedForce(newForce);
}
}
//PxVec3_From_Vec3V(linVel0, l0);
//PxVec3_From_Vec3V(angVel0, a0);
//PX_ASSERT(l0.isFinite());
//PX_ASSERT(a0.isFinite());
// Write back
V3StoreU(linVel0, b0.linearVelocity);
V3StoreU(angVel0, b0.angularVelocity);
PX_ASSERT(currPtr == last);
}
void solveContactCoulomb_BStatic(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
PxcSolverBody& b0 = *desc.bodyA;
Vec3V linVel0 = V3LoadA(b0.linearVelocity);
Vec3V angVel0 = V3LoadA(b0.angularVelocity);
PxcSolverContactCoulombHeader* firstHeader = (PxcSolverContactCoulombHeader*)desc.constraint;
const PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;//getConstraintLength(desc);
//hopefully pointer aliasing doesn't bite.
const PxU8* PX_RESTRICT currPtr = desc.constraint;
const FloatV zero = FZero();
while(currPtr < last)
{
PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (PxcSolverContactCoulombHeader*)currPtr;
currPtr += sizeof(PxcSolverContactCoulombHeader);
const PxU32 numNormalConstr = hdr->numNormalConstr;
PxcSolverContact* PX_RESTRICT contacts = (PxcSolverContact*)currPtr;
Ps::prefetchLine(contacts);
currPtr += numNormalConstr * sizeof(PxcSolverContact);
PxF32* appliedImpulse = (PxF32*) (((PxU8*)hdr) + hdr->frictionOffset + sizeof(PxcSolverFrictionHeader));
Ps::prefetchLine(appliedImpulse);
const Vec3V normal = hdr->getNormal();
const FloatV invMassDom0 = FLoad(hdr->dominance0);
FloatV normalVel1 = V3Dot(normal, linVel0);
const Vec3V delLinVel0 = V3Scale(normal, invMassDom0);
FloatV accumDeltaF = zero;
//FloatV accumImpulse = zero;
for(PxU32 i=0;i<numNormalConstr;i++)
{
PxcSolverContact& c = contacts[i];
Ps::prefetchLine(&contacts[i+1]);
//const Vec4V normalXYZ_velMultiplierW = c.normalXYZ_velMultiplierW;
const Vec4V raXnXYZ_appliedForceW = c.raXnXYZ_appliedForceW;
const Vec4V rbXnXYZ_velMultiplierW = c.rbXnXYZ_velMultiplierW;
//const Vec3V normal = c.normal;
//const Vec3V normal = Vec3V_From_Vec4V(normalXYZ_velMultiplierW);
const Vec3V raXn = Vec3V_From_Vec4V(raXnXYZ_appliedForceW);
const FloatV appliedForce = V4GetW(raXnXYZ_appliedForceW);
const FloatV velMultiplier = V4GetW(rbXnXYZ_velMultiplierW);
//const FloatV velMultiplier = V4GetW(normalXYZ_velMultiplierW);
const Vec3V delAngVel0 = Vec3V_From_Vec4V(c.delAngVel0_InvMassADom);
const FloatV targetVel = c.getTargetVelocity();
const FloatV nScaledBias = FNeg(c.getScaledBias());
const FloatV maxImpulse = c.getMaxImpulse();
//Compute the normal velocity of the constraint.
//const FloatV normalVel1 = V3Dot(normal, linVel0);
const FloatV normalVel2 = V3Dot(raXn, angVel0);
const FloatV normalVel = FAdd(normalVel1, normalVel2);
//const FloatV unbiasedErr = FMul(targetVel, velMultiplier);
const FloatV biasedErr = FMulAdd(targetVel, velMultiplier, nScaledBias);
// still lots to do here: using loop pipelining we can interweave this code with the
// above - the code here has a lot of stalls that we would thereby eliminate
const FloatV _deltaF = FMax(FNegMulSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
const FloatV _newForce = FAdd(appliedForce, _deltaF);
const FloatV newForce = FMin(_newForce, maxImpulse);
const FloatV deltaF = FSub(newForce, appliedForce);
//linVel0 = V3MulAdd(delLinVel0, deltaF, linVel0);
normalVel1 = FScaleAdd(invMassDom0, deltaF, normalVel1);
angVel0 = V3ScaleAdd(delAngVel0, deltaF, angVel0);
accumDeltaF = FAdd(accumDeltaF, deltaF);
c.setAppliedForce(newForce);
Ps::aos::FStore(newForce, &appliedImpulse[i]);
Ps::prefetchLine(&appliedImpulse[i], 128);
//accumImpulse = FAdd(accumImpulse, newAppliedForce);
}
linVel0 = V3ScaleAdd(delLinVel0, accumDeltaF, linVel0);
//hdr->setAccumlatedForce(accumImpulse);
}
// Write back
V3StoreU(linVel0, b0.linearVelocity);
V3StoreU(angVel0, b0.angularVelocity);
PX_ASSERT(currPtr == last);
}
void RTree::refitAllStaticTree(CallbackRefit& cb, PxBounds3* retBounds)
{
PxU8* treeNodes8 = PX_IS_X64 ? CAST_U8(get64BitBasePage()) : CAST_U8((mFlags & IS_DYNAMIC) ? NULL : mPages);
// since pages are ordered we can scan back to front and the hierarchy will be updated
for (PxI32 iPage = PxI32(mTotalPages)-1; iPage>=0; iPage--)
{
RTreePage& page = mPages[iPage];
for (PxU32 j = 0; j < RTREE_PAGE_SIZE; j++)
{
if (page.isEmpty(j))
continue;
if (page.isLeaf(j))
{
Vec3V childMn, childMx;
cb.recomputeBounds(page.ptrs[j]-1, childMn, childMx); // compute the bound around triangles
PxVec3 mn3, mx3;
V3StoreU(childMn, mn3);
V3StoreU(childMx, mx3);
page.minx[j] = mn3.x; page.miny[j] = mn3.y; page.minz[j] = mn3.z;
page.maxx[j] = mx3.x; page.maxy[j] = mx3.y; page.maxz[j] = mx3.z;
} else
{
const RTreePage* child = (const RTreePage*)(treeNodes8 + page.ptrs[j]);
PX_COMPILE_TIME_ASSERT(RTREE_PAGE_SIZE == 4);
bool first = true;
for (PxU32 k = 0; k < RTREE_PAGE_SIZE; k++)
{
if (child->isEmpty(k))
continue;
if (first)
{
page.minx[j] = child->minx[k]; page.miny[j] = child->miny[k]; page.minz[j] = child->minz[k];
page.maxx[j] = child->maxx[k]; page.maxy[j] = child->maxy[k]; page.maxz[j] = child->maxz[k];
first = false;
} else
{
page.minx[j] = PxMin(page.minx[j], child->minx[k]);
page.miny[j] = PxMin(page.miny[j], child->miny[k]);
page.minz[j] = PxMin(page.minz[j], child->minz[k]);
page.maxx[j] = PxMax(page.maxx[j], child->maxx[k]);
page.maxy[j] = PxMax(page.maxy[j], child->maxy[k]);
page.maxz[j] = PxMax(page.maxz[j], child->maxz[k]);
}
}
}
}
}
if (retBounds)
{
RTreeNodeQ bound1;
for (PxU32 ii = 0; ii<mNumRootPages; ii++)
{
mPages[ii].computeBounds(bound1);
if (ii == 0)
{
retBounds->minimum = PxVec3(bound1.minx, bound1.miny, bound1.minz);
retBounds->maximum = PxVec3(bound1.maxx, bound1.maxy, bound1.maxz);
} else
{
retBounds->minimum = retBounds->minimum.minimum(PxVec3(bound1.minx, bound1.miny, bound1.minz));
retBounds->maximum = retBounds->maximum.maximum(PxVec3(bound1.maxx, bound1.maxy, bound1.maxz));
}
}
}
#ifdef PX_CHECKED
validate(&cb);
#endif
}
