Point3 *CalcSphereCenter(Point3 *v0, Point3 *v1, Point3 *v2, Point3 *v3,Point3 *c)
{
Vector3 d1, d2, d3;
Plane p1,p2,p3;
V3Add(v0, v1, &d1);
V3Add(v0, v2, &d2);
V3Add(v0, v3, &d3);
d1.x/=2; d1.y/=2; d1.z/=2;
d2.x/=2; d2.y/=2; d2.z/=2;
d3.x/=2; d3.y/=2; d3.z/=2;
V3Sub(v0, v1, &(p1.N));
V3Sub(v0, v2, &(p2.N));
V3Sub(v0, v3, &(p3.N));
V3Normalize(&(p1.N));
V3Normalize(&(p2.N));
V3Normalize(&(p3.N));
p1.off = V3Dot(&(p1.N), &d1);
p2.off = V3Dot(&(p2.N), &d2);
p3.off = V3Dot(&(p3.N), &d3);
if(CalcPlaneInter(&p1, &p2, &p3, c)) return c;
else return NULL;
}
boolean PointBelongtoPlane(Point3 *p, Plane *pl)
{ double v;
v= -pl->off+V3Dot(p,&(pl->N));
if(fabs(v)<=0.001) return 1;
return 0;
}
/*
** Search for an intersection between a facet and a ray
*/
boolean hit_ray_quad_gg(RAY *Ray, QUAD *Quad, HIT *Hit)
{
Point3 Point;
/* if the ray is parallel to the facet, there is no intersection */
Hit->Distance = V3Dot (&(Ray->Vector), &(Quad->Normal));
if (ABS(Hit->Distance) < EPSILON) return (FALSE);
/* compute ray intersection with the plane of the facet */
V3Sub (&(Quad->A), &(Ray->Point), &Point);
Hit->Distance = V3Dot (&Point, &(Quad->Normal)) / Hit->Distance;
V3_LIN (Hit->Point, Ray->Point, Hit->Distance, Ray->Vector);
/* is the intersection point inside the facet */
return (point_in_quad(Quad, Hit));
}
Point3 *CalcLinePlaneInter(Line *l, Plane *p, Point3 *c)
{
double t,t1;
t = - p->off + V3Dot(&(l->Lu),&(p->N));
t1 = V3Dot(&(l->Lv),&(p->N));
if(t1==0) return NULL;
t=-(t/t1);
c->x=l->Lv.x * t;
c->y=l->Lv.y * t;
c->z=l->Lv.z * t;
V3Add(c,&(l->Lu),c);
SI.TestedPoint++;
return c;
}
Plane *CalcMiddlePlane(Point3 *p1, Point3 *p2, Plane *p)
{Point3 m0;
V3Add(p1,p2,&m0);
m0.x/=2;m0.y/=2;m0.z/=2;
V3Sub(p1,p2,&(p->N));
V3Normalize(&(p->N));
p->off = V3Dot(&m0,&(p->N));
return p;
}
int Ray_BlobAddPlane(Object *obj, Vec3 *pt, Vec3 *dir,
double dist, double field)
{
Bloblet *be;
BlobData *blob = obj->data.blob;
Vec3 Pt;
assert(obj != NULL);
assert(blob != NULL);
if ((be = (Bloblet *)Malloc(sizeof(Bloblet))) == NULL)
return 0;
be->next = NULL;
V3Copy(&be->loc, pt);
V3Copy(&be->dir, dir);
be->rad = dist;
be->field = field;
/*
* Normalize the normal vector.
* Invalid normals should be checked for during parse.
*/
V3Normalize(&be->dir);
/* Get "d" coeff. for plane that bounds plane's interval. */
Pt.x = be->dir.x * be->rad;
Pt.y = be->dir.y * be->rad;
Pt.z = be->dir.z * be->rad;
be->d1 = -V3Dot(&be->dir, &Pt);
/*
* Pre-compute constants for the density eq. in standard form.
* Radii that are too small shoud be checked for during parse.
*/
be->rsq = be->rad * be->rad;
be->r2 = - (2.0 * be->field) / be->rsq;
be->r4 = be->field / (be->rsq * be->rsq);
be->type = BLOB_PLANE;
/* Add the new plane element to the blob. */
if (blob->elems != NULL)
{
Bloblet *lastbe;
for (lastbe = blob->elems; lastbe->next != NULL; lastbe = lastbe->next)
; /* Seek last element added to list. */
/* Append our new one. */
lastbe->next = be;
}
else
blob->elems = be;
return 1;
}
int IsInsideColorTriangle(Object *obj, Vec3 *P)
{
ColorTriangleData *tri = obj->data.colortri;
Vec3 Pt, N;
Pt.x = P->x - tri->pts[0];
Pt.y = P->y - tri->pts[1];
Pt.z = P->z - tri->pts[2];
N.x = tri->pnorm[0];
N.y = tri->pnorm[1];
N.z = tri->pnorm[2];
if(V3Dot(&Pt, &N) > 0.0 )
return (obj->flags & OBJ_FLAG_INVERSE); /* not inside */
return ( ! (obj->flags & OBJ_FLAG_INVERSE)); /* inside */
}
Plane *CalcPlane(Vector3 *p0, Vector3 *p1, Vector3 *p2, Plane *p )
{
Vector3 v1,v2;
Vector3 *N;
N=&(p->N);
V3Sub(p1,p0,&v1);
V3Sub(p2,p0,&v2);
V3Cross(&v1,&v2,N);
if(N->x==0.0 && N->y==0.0 && N->z==0.0) return NULL;
V3Normalize(N);
p->off=V3Dot(N,p0);
return p;
}
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 PxcArticulationHelper::getImpulseSelfResponse(const PxcFsData& matrix,
PxU32 linkID0,
const PxcSIMDSpatial& impulse0,
PxcSIMDSpatial& deltaV0,
PxU32 linkID1,
const PxcSIMDSpatial& impulse1,
PxcSIMDSpatial& deltaV1)
{
PX_ASSERT(linkID0 != linkID1);
const PxcFsRow* rows = getFsRows(matrix);
const PxcFsRowAux* aux = getAux(matrix);
const PxcFsJointVectors* jointVectors = getJointVectors(matrix);
PX_UNUSED(aux);
PxcSIMDSpatial& dV0 = deltaV0,
& dV1 = deltaV1;
// standard case: parent-child limit
if(matrix.parent[linkID1] == linkID0)
{
const PxcFsRow& r = rows[linkID1];
const PxcFsJointVectors& j = jointVectors[linkID1];
Vec3V lZ = V3Neg(impulse1.linear),
aZ = V3Neg(impulse1.angular);
Vec3V sz = V3Add(aZ, V3Cross(lZ, j.jointOffset));
lZ = V3Sub(lZ, V3ScaleAdd(r.DSI[0].linear, V3GetX(sz), V3ScaleAdd(r.DSI[1].linear, V3GetY(sz), V3Scale(r.DSI[2].linear, V3GetZ(sz)))));
aZ = V3Sub(aZ, V3ScaleAdd(r.DSI[0].angular, V3GetX(sz), V3ScaleAdd(r.DSI[1].angular, V3GetY(sz), V3Scale(r.DSI[2].angular, V3GetZ(sz)))));
aZ = V3Add(aZ, V3Cross(j.parentOffset, lZ));
lZ = V3Sub(impulse0.linear, lZ);
aZ = V3Sub(impulse0.angular, aZ);
dV0 = getImpulseResponseSimd(matrix, linkID0, lZ, aZ);
Vec3V aV = dV0.angular;
Vec3V lV = V3Sub(dV0.linear, V3Cross(j.parentOffset, aV));
Vec3V n = V3Add(V3Merge(V3Dot(r.DSI[0].linear, lV), V3Dot(r.DSI[1].linear, lV), V3Dot(r.DSI[2].linear, lV)),
V3Merge(V3Dot(r.DSI[0].angular, aV), V3Dot(r.DSI[1].angular, aV), V3Dot(r.DSI[2].angular, aV)));
n = V3Add(n, M33MulV3(r.D, sz));
lV = V3Sub(lV, V3Cross(j.jointOffset, n));
aV = V3Sub(aV, n);
dV1 = PxcSIMDSpatial(lV, aV);
}
else
getImpulseResponseSlow(matrix, linkID0, impulse0, deltaV0, linkID1, impulse1, deltaV1);
#if PXC_ARTICULATION_DEBUG_VERIFY
PxcSIMDSpatial dV0_, dV1_;
PxcFsGetImpulseSelfResponse(matrix, linkID0, impulse0, dV0_, linkID1, impulse1, dV1_);
PX_ASSERT(almostEqual(dV0_, dV0, 1e-3f));
PX_ASSERT(almostEqual(dV1_, dV1, 1e-3f));
#endif
}
boolean RightSide(Plane *p, Point3 *v)
{
if(V3Dot(v,&(p->N)) > p->off + EPSILON) return TRUE;
else return FALSE;
}
Object *Ray_MakeBlob(PARAMS *par)
{
Object *obj;
int token, num_elems, i;
PARAMS *p;
BlobData *b;
Bloblet *be, *new_be, *hemi1, *hemi2, *cyl;
static Vec3 pt;
if((obj = NewObject()) == NULL)
return NULL;
/* evaluate parameters */
p = par;
while(1)
{
Eval_Params(p);
for(i = p->more; i >= 0; i--)
p = p->next;
if(p == NULL)
break;
p = p->next; /* skip element type delimiter */
}
/* Allocate the main blob data structure... */
b = (BlobData *)Malloc(sizeof(BlobData));
b->nrefs = 1;
/* Get threshold... */
b->threshold = par->V.x;
b->solver = 0;
b->bound = NULL;
/* Get blob elements... */
be = NULL;
num_elems = 0;
while((par = par->next) != NULL)
{
token = par->type;
/* Get blob element... */
switch(token)
{
case BLOB_SPHERE:
case BLOB_PLANE:
case BLOB_CYLINDER:
for(i = 0; i < 3; i++)
{
/* Allocate a "Bloblet" structure. */
new_be = (Bloblet *)Malloc(sizeof(Bloblet));
new_be->next = NULL;
new_be->field = 1.0; /* Default field strength, if not given. */
if(be == NULL) /* First one, start the list. */
{
be = new_be;
b->elems = be;
}
else /* Add to the list. */
{
be->next = new_be;
be = be->next;
}
num_elems++;
if(token != BLOB_CYLINDER)
break;
/* Save references to the hemisphere sub-elements for cylinder. */
if(i == 0) cyl = new_be;
else if(i == 1) hemi1 = new_be;
else hemi2 = new_be;
}
par = par->next;
break;
default:
break;
}
/* Get parameters... */
switch(token)
{
case BLOB_SPHERE:
V3Copy(&be->loc, &par->V);
par = par->next;
be->rad = par->V.x;
if(par->more) /* Optional field strength was specified, also. */
{
par = par->next;
be->field = par->V.x;
}
/* Pre-compute constants for the density eq. in standard form. */
/* Radii that are too small shoud be checked for during parse. */
be->rsq = be->rad * be->rad;
be->r2 = - (2.0 * be->field) / be->rsq;
be->r4 = be->field / (be->rsq * be->rsq);
be->type = BLOB_SPHERE;
continue;
case BLOB_PLANE:
V3Copy(&be->loc, &par->V);
par = par->next;
V3Copy(&be->dir, &par->V);
par = par->next;
be->rad = par->V.x;
if(par->more) /* Optional field strength was specified, also. */
{
par = par->next;
be->field = par->V.x;
}
/* Normalize the normal vector. */
/* Invalid normals should be checked for during parse. */
V3Normalize(&be->dir);
/* Get "d" coeff. for plane that bounds plane's interval. */
pt.x = be->dir.x * be->rad;
pt.y = be->dir.y * be->rad;
pt.z = be->dir.z * be->rad;
be->d1 = -V3Dot(&be->dir, &pt);
/* Pre-compute constants for the density eq. in standard form. */
/* Radii that are too small shoud be checked for during parse. */
be->rsq = be->rad * be->rad;
be->r2 = - (2.0 * be->field) / be->rsq;
be->r4 = be->field / (be->rsq * be->rsq);
be->type = BLOB_PLANE;
continue;
case BLOB_CYLINDER:
V3Copy(&cyl->loc, &par->V);
par = par->next;
V3Copy(&pt, &par->V); /* end point for cylinder */
par = par->next;
cyl->rad = par->V.x;
if(par->more) /* Optional field strength was specified, also. */
{
par = par->next;
cyl->field = par->V.x;
}
/* Pre-compute constants for the density eq. in standard form. */
/* Radii that are too small shoud be checked for during parse. */
cyl->rsq = cyl->rad * cyl->rad;
cyl->r2 = -( 2.0 * cyl->field) / cyl->rsq;
cyl->r4 = cyl->field / (cyl->rsq * cyl->rsq);
/* Get offset vector of cylinder. */
V3Sub(&cyl->d, &pt, &cyl->loc);
/* Get cylinder length squared & length, while we're at it... */
/* Zero length offset vectors shoud be checked for during parse. */
cyl->lsq = V3Dot(&cyl->d, &cyl->d);
cyl->len = sqrt(cyl->lsq);
V3Copy(&cyl->dir, &cyl->d);
V3Normalize(&cyl->dir);
/* Get "d" coeffs. for planes at cylinder ends. */
cyl->d1 = -V3Dot(&cyl->dir, &cyl->loc);
cyl->d2 = -V3Dot(&cyl->dir, &pt);
/* Precompute location dot offset_vector constant. */
cyl->l_dot_d = V3Dot(&cyl->loc, &cyl->d);
cyl->type = BLOB_CYLINDER;
/* Set up the hemispheres for the ends of the cylinder. */
V3Copy(&hemi1->loc, &cyl->loc);
V3Sub(&hemi1->dir, &pt, &hemi1->loc);
V3Normalize(&hemi1->dir);
hemi1->rad = cyl->rad;
hemi1->rsq = cyl->rsq;
hemi1->field = cyl->field;
hemi1->r2 = cyl->r2;
hemi1->r4 = cyl->r4;
hemi1->type = BLOB_HEMISPHERE;
V3Copy(&hemi2->loc, &pt);
V3Sub(&hemi2->dir, &cyl->loc, &hemi2->loc);
V3Normalize(&hemi2->dir);
hemi2->rad = cyl->rad;
hemi2->rsq = cyl->rsq;
hemi2->field = cyl->field;
hemi2->r2 = cyl->r2;
hemi2->r4 = cyl->r4;
hemi2->type = BLOB_HEMISPHERE;
continue;
default:
break;
}
break;
}
/*
* Blobs must have at least one element, this should be checked
* for during parse.
*/
/* Some assembly required... */
b->hits = (BlobHit **)Malloc(sizeof(BlobHit *) * num_elems * 2);
for (i = 0; i < num_elems * 2; i++)
b->hits[i] = (BlobHit *)Malloc(sizeof(BlobHit));
obj->data.blob = b;
obj->procs = &blob_procs;
return obj;
}
/*
* calc_intervals() - Cycle through all blob elements checking for
* ray/field-of-influence intersections. For every element hit, add
* two intervals to a list, "intervals", in order of distance with the
* closest at the top of the list. Returns 1 if a list was created,
* or zero if not.
*/
static int calc_intervals(BlobData *blob, BlobHit **intervals)
{
Bloblet *be;
BlobHit **be_hit_array, *first_bi, *cur, *prev, *new_hit;
double a, b, c, d, t1, t2;
int i;
first_bi = NULL;
be_hit_array = blob->hits;
dx = D.x;
dy = D.y;
dz = D.z;
for (be = blob->elems; be != NULL; be = be->next)
{
ox = B.x - be->loc.x;
oy = B.y - be->loc.y;
oz = B.z - be->loc.z;
if (be->type == BLOB_CYLINDER)
{
Vec3 cK, cD;
/*Calculate the ray's base location in the object's coordinate system.*/
t2 = V3Dot(&B, &be->d) - V3Dot(&be->loc, &be->d);
t1 = t2 / be->lsq; /* precomputed length squared */
cK.x = B.x - be->loc.x - t1 * be->d.x;
cK.y = B.y - be->loc.y - t1 * be->d.y;
cK.z = B.z - be->loc.z - t1 * be->d.z;
t2 = V3Dot(&D, &be->d);
t1 = t2 / be->lsq;
cD.x = D.x - t1 * be->d.x;
cD.y = D.y - t1 * be->d.y;
cD.z = D.z - t1 * be->d.z;
a = V3Dot(&cD, &cD);
b = 2.0 * V3Dot(&cD, &cK);
c = V3Dot(&cK, &cK) - be->rsq;
d = b * b - 4.0 * a * c;
if (d < 0.0)
continue; /* no roots, we miss */
d = sqrt(d);
t2 = (-b + d) / (2.0 * a);
t1 = (-b - d) / (2.0 * a);
/* Make sure that t1 is the smaller... */
if(t1 > t2)
{
d = t1;
t1 = t2;
t2 = d;
}
/* See if interval is within area of interest... */
if (t1 < ct.tmin)
t1 = ct.tmin;
if (t2 < ct.tmin)
continue;
/* Factor in the end planes of cylinder. */
{
double num, den, t3;
/* Intersect plane at cylinder base. */
den = be->dir.x * dx + be->dir.y * dy + be->dir.z * dz;
num = V3Dot(&be->dir, &B) + be->d1;
if (fabs(den) > EPSILON)
{
if (ISZERO(num))
{
if (den < 0.0 || t2 < ct.tmin)
continue;
if (t1 < ct.tmin)
t1 = ct.tmin;
}
else
{
t3 = -num / den;
if (num < 0.0)
{
if (t2 < t3 || t3 < 0.0)
continue; /* Miss cylinder. */
if (t1 < t3)
t1 = t3;
}
else if(t3 >= 0.0)
{
if (t1 > t3)
continue; /* Miss cylinder. */
if (t2 > t3)
t2 = t3;
}
}
/* Intersect plane at cylinder end point. */
num = V3Dot(&be->dir, &B) + be->d2;
if (ISZERO(num))
{
if (den > 0.0 || t2 < ct.tmin)
continue;
if (t1 < ct.tmin)
t1 = ct.tmin;
}
else
{
t3 = - (num) / den;
if(num > 0.0)
{
if (t2 < t3 || t3 < 0.0)
continue; /* Miss cylinder. */
if (t1 < t3)
t1 = t3;
}
else if(t3 >= 0.0)
{
if (t1 > t3)
continue; /* Miss cylinder. */
if (t2 > t3)
t2 = t3;
}
}
}
else /* Ray is paralell to cyl base & end planes. */
{
if (num < 0.0)
continue;
num = V3Dot(&be->dir, &B) + be->d2;
if (num > 0.0)
continue;
}
}
}
else if (be->type == BLOB_PLANE)
{
double num, den;
/* Intersect plane that bounds interval for plane. */
den = be->dir.x * dx + be->dir.y * dy + be->dir.z * dz;
num = be->dir.x * ox + be->dir.y * oy + be->dir.z * oz
+ be->d1;
if (fabs(den) > EPSILON)
{
t1 = - (num) / den;
if (num > 0.0)
{
if (t1 < ct.tmin)
continue; /* Miss plane. */
t2 = ct.tmax;
}
else
{
if (t1 < ct.tmin)
{
t1 = ct.tmin;
t2 = ct.tmax;
}
else
{
t2 = t1;
t1 = ct.tmin;
}
}
}
else
{
if (num > 0.0)
continue;
t1 = ct.tmin;
t2 = ct.tmax;
}
}
else /* Spheres or hemi-spheres. */
{
/*
* Test for intersection between ray and sphere.
*/
a = - (ox * dx + oy * dy + oz * dz);
d = be->rsq - (( ox * ox + oy * oy + oz * oz) - a * a);
if (d < 0.0)
continue;
d = sqrt(d);
t2 = a + d;
t1 = a - d;
/* Make sure that t1 is the smaller... */
if (t1 > t2)
{
d = t1;
t1 = t2;
t2 = d;
}
/* See if interval is within area of interest... */
if (t1 < ct.tmin)
t1 = ct.tmin;
if (t2 < ct.tmin)
continue;
/* Factor in plane part if this is a hemi-sphere. */
if (be->type == BLOB_HEMISPHERE)
{
double t3, num, den;
/* Intersect plane. */
den = be->dir.x * dx + be->dir.y * dy + be->dir.z * dz;
num = be->dir.x * ox + be->dir.y * oy + be->dir.z * oz;
if (fabs(den) > EPSILON)
{
if (ISZERO(num))
{
if (den > 0.0 || t2 < ct.tmin)
continue;
if (t1 < ct.tmin)
t1 = ct.tmin;
}
else
{
t3 = - (num) / den;
if (num > 0.0)
{
if (t2 < t3 || t3 < 0.0)
continue; /* Miss hemi-sphere. */
if (t1 < t3)
t1 = t3;
}
else if (t3 >= 0.0)
{
if (t1 > t3)
continue; /* Miss hemi-sphere. */
if (t2 > t3)
t2 = t3;
}
}
}
else /* Ray is paralell to plane, just check against ray base. */
if (num > 0.0)
continue;
}
} /* End of else Spheres or Hemi-Spheres. */
/* Make sure that t1 is the smaller... */
if (t1 > t2)
{
d = t1;
t1 = t2;
t2 = d;
}
/* See if interval is within area of interest... */
if (t1 > ct.tmax || t2 <= ct.tmin)
continue;
/* Reject any intervals that are too small. */
if (fabs(t1 - t2) < MIN_INTERVAL_SIZE)
continue;
/* Truncate any parts of interval that are out of bounds... */
if (t1 < ct.tmin)
t1 = ct.tmin;
if (t2 > ct.tmax)
t2 = ct.tmax;
for (i = 0; i < 2; i++)
{
new_hit = *be_hit_array++;
if (i == 0)
{
new_hit->t = t1;
new_hit->entering = 1;
}
else
{
new_hit->t = t2;
new_hit->entering = 0;
}
new_hit->next = NULL;
new_hit->be = be;
/* Build hit list in order from closest to farthest. */
cur = first_bi;
prev = NULL;
while (cur != NULL)
{
if (new_hit->t <= cur->t)
{
new_hit->next = cur;
if (prev != NULL) /* Inserting some where inside list. */
prev->next = new_hit;
else /* Put on top of list. */
first_bi = new_hit;
break;
}
prev = cur;
cur = cur->next;
}
if (cur == NULL) /* End of list was reached. */
{
if (prev == NULL) /* Nothing was in list, first hit. */
first_bi = new_hit;
else
prev->next = new_hit;
}
} /* end of add two intervals loop */
} /* end of blob elements loop */
*intervals = first_bi;
if (first_bi == NULL)
return 0; /* No intervals in list - No blob intersections. */
return 1;
} /* end calc_intervals() */
void calc_substitutions(BlobHit *bi)
{
Bloblet *be;
double r2, r4, a, b, c, d;
/*
* Whenever a new interval is entered, compute the density
* equation coefficients for its associated blob element
* and store in blob elements's c[] array for later reference.
*/
be = bi->be;
ox = B.x - be->loc.x;
oy = B.y - be->loc.y;
oz = B.z - be->loc.z;
r2 = be->r2;
r4 = be->r4;
if (be->type == BLOB_CYLINDER)
{
double dx0, dy0, dz0, t;
/*
* Transform ray base to point relative to cylinder's
* symetrical axis.
*/
t = (V3Dot(&B, &be->d) - be->l_dot_d) / be->lsq;
ox -= t * be->d.x;
oy -= t * be->d.y;
oz -= t * be->d.z;
/* Scale the ray's direction agaist the cylinder's direction. */
t = (dx * be->d.x + dy * be->d.y + dz * be->d.z) / be->lsq;
dx0 = dx - t * be->d.x;
dy0 = dy - t * be->d.y;
dz0 = dz - t * be->d.z;
a = dx0 * dx0 + dy0 * dy0 + dz0 * dz0;
b = ox * dx0 + oy * dy0 + oz * dz0;
c = ox * ox + oy * oy + oz * oz;
d = 4.0 * r4 * b;
be->c[4] = r4 * a * a;
be->c[3] = a * d;
be->c[2] = d * b + a *(2.0 * r4 * c + r2);
be->c[1] = c * d + 2.0 * r2 * b;
be->c[0] = c * (r4 * c + r2) + be->field;
}
else if (be->type == BLOB_PLANE)
{
double A, B, C, p0, p1;
A = be->dir.x;
B = be->dir.y;
C = be->dir.z;
p1 = A * dx + B * dy + C * dz;
p0 = A * ox + B * oy + C * oz;
be->c[4] = 0.0;
be->c[3] = 0.0;
be->c[2] = r4 * p1 * p1;
be->c[1] = 2.0 * r4 * p0 * p1 + r2 * p1;
be->c[0] = r4 * p0 * p0 + r2 * p0 + be->field;
}
else /* Spheres and hemispheres. */
{
b = ox * dx + oy * dy + oz * dz;
c = ox * ox + oy * oy + oz * oz;
d = 4.0 * r4 * b;
be->c[4] = r4;
be->c[3] = d;
be->c[2] = d * b + 2.0 * r4 * c + r2;
be->c[1] = c * d + 2.0 * r2 * b;
be->c[0] = c * (r4 * c + r2) + be->field;
}
} /* end of calc_substitutions() */
void eval_vdot(Expr *expr)
{
expr->l->fn(expr->l);
expr->r->fn(expr->r);
expr->v.x = V3Dot(&expr->l->v, &expr->r->v);
}
void solveFriction_BStatic(const PxSolverConstraintDesc& desc, SolverContext& /*cache*/)
{
PxSolverBody& b0 = *desc.bodyA;
Vec3V linVel0 = V3LoadA(b0.linearVelocity);
Vec3V angState0 = V3LoadA(b0.angularState);
PxU8* PX_RESTRICT currPtr = desc.constraint;
const PxU8* PX_RESTRICT last = currPtr + getConstraintLength(desc);
while(currPtr < last)
{
const SolverFrictionHeader* PX_RESTRICT frictionHeader = reinterpret_cast<SolverFrictionHeader*>(currPtr);
const PxU32 numFrictionConstr = frictionHeader->numFrictionConstr;
const PxU32 numNormalConstr = frictionHeader->numNormalConstr;
const PxU32 numFrictionPerPoint = numFrictionConstr/numNormalConstr;
currPtr +=sizeof(SolverFrictionHeader);
PxF32* appliedImpulse = reinterpret_cast<PxF32*>(currPtr);
currPtr +=frictionHeader->getAppliedForcePaddingSize();
SolverContactFriction* PX_RESTRICT frictions = reinterpret_cast<SolverContactFriction*>(currPtr);
currPtr += numFrictionConstr * sizeof(SolverContactFriction);
const FloatV invMass0 = FLoad(frictionHeader->invMass0D0);
const FloatV angD0 = FLoad(frictionHeader->angDom0);
//const FloatV angD1 = FLoad(frictionHeader->angDom1);
const FloatV staticFriction = frictionHeader->getStaticFriction();
for(PxU32 i=0, j = 0;i<numFrictionConstr;j++)
{
for(PxU32 p = 0; p < numFrictionPerPoint; p++, i++)
{
SolverContactFriction& 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 = FLoad(f.targetVel);
//const FloatV normalImpulse = contacts[f.contactIndex].getAppliedForce();
const FloatV normalImpulse = FLoad(appliedImpulse[j]);
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, angState0);
const FloatV t0Vel = FAdd(t0Vel1, t0Vel2);
const Vec3V delangState0 = V3Scale(raXt0, angD0);
const Vec3V delLinVel0 = V3Scale(t0, invMass0);
// 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 tmp = FNegScaleSub(targetVel,velMultiplier,appliedForce);
FloatV newForce = FScaleAdd(t0Vel, velMultiplier, tmp);
newForce = FClamp(newForce, nMaxFriction, maxFriction);
const FloatV deltaF = FSub(newForce, appliedForce);
linVel0 = V3ScaleAdd(delLinVel0, deltaF, linVel0);
angState0 = V3ScaleAdd(delangState0, deltaF, angState0);
f.setAppliedForce(newForce);
}
}
}
// Write back
V3StoreA(linVel0, b0.linearVelocity);
V3StoreA(angState0, b0.angularState);
PX_ASSERT(currPtr == last);
}
int FindClosestIntersection(Object *first_obj, HitData *hits)
{
Object *obj, *closest_obj;
struct BBQ *first_bbox_queued, *last_bbox_queued, *bbq;
double closest_t;
int entering;
last_bbox_queued = NULL;
first_bbox_queued = NULL;
ct.calc_all = 0;
closest_t = HUGE;
closest_obj = NULL;
obj = first_obj;
while(obj != NULL)
{
if( ! ((ct.ray_flags & RAY_SHADOW) &&
((obj->flags & OBJ_FLAG_NO_SHADOW) ||
((obj == ct.baseobj) && (obj->flags & OBJ_FLAG_NO_SELF_INTERSECT))))
)
{
if((obj->procs->Intersect)(obj, hits))
{
if(obj->procs->type == OBJ_BBOX)
{
/*
* Place bounding boxes in queue to be tested
* after the non-BBox objects in this list are tested.
*/
bbq = fetch_bbq();
bbq->bbox = obj->data.bbox;
bbq->t = hits->t;
if(last_bbox_queued == NULL)
first_bbox_queued = bbq;
else
last_bbox_queued->next = bbq;
last_bbox_queued = bbq;
}
else if(hits->t < closest_t)
{
closest_obj = hits->obj;
closest_t = hits->t;
entering = hits->entering;
}
}
}
obj = obj->next;
if(obj == NULL)
{
/*
* End of object list was reached. If there are
* bounding boxes, pull them from the queue until
* one that has a "t" value that is closer than the
* closest object hit, if any, is found and recycle.
* "obj" will point to a new list if a closer BBox
* is found.
*/
while(first_bbox_queued != NULL)
{
/* Pull first bounding box from the queue... */
bbq = first_bbox_queued;
first_bbox_queued = first_bbox_queued->next;
if(first_bbox_queued == NULL) /* This was the only one... */
last_bbox_queued = NULL; /* ...Clear the queue. */
/* Get potential new object list to test... */
obj = bbq->bbox->objects;
/* Flag this queue element as "free" in the global pool. */
bbq->bbox = NULL;
/* See if BBox is closer than closest object... */
if(bbq->t < closest_t)
break; /* BBox is closer, recycle with the new list... */
/* BBox is not closer, discard the new list... */
obj = NULL;
/* Move on to next BBox, if any. */
}
}
}
if(closest_obj != NULL)
{
ct.objhit = closest_obj;
ct.t = closest_t;
ct.entering = entering;
ct.Q.x = ct.B.x + closest_t * ct.D.x;
ct.Q.y = ct.B.y + closest_t * ct.D.y;
ct.Q.z = ct.B.z + closest_t * ct.D.z;
closest_obj->procs->CalcNormal(closest_obj, &ct.Q, &ct.N);
if(V3Dot(&ct.N, &ct.D) > 0.0)
{
ct.N.x = -ct.N.x;
ct.N.y = -ct.N.y;
ct.N.z = -ct.N.z;
}
return 1;
}
return 0;
}
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;
}
int Ray_BlobAddCylinder(Object *obj, Vec3 *pt1, Vec3 *pt2,
double radius, double field)
{
Bloblet *hemi1, *hemi2, *cyl;
BlobData *blob = obj->data.blob;
assert(obj != NULL);
assert(blob != NULL);
/*
* Cylindrical fields consist of three Bloblet elements.
* A cylinder and two hemispheres.
*/
/* Allocate the elements. */
if ((cyl = (Bloblet *)Malloc(sizeof(Bloblet))) == NULL)
return 0;
if ((hemi1 = (Bloblet *)Malloc(sizeof(Bloblet))) == NULL)
{
Free(cyl, sizeof(Bloblet));
return 0;
}
if ((hemi2 = (Bloblet *)Malloc(sizeof(Bloblet))) == NULL)
{
Free(hemi1, sizeof(Bloblet));
Free(cyl, sizeof(Bloblet));
return 0;
}
/* Link them together. */
cyl->next = hemi1;
hemi1->next = hemi2;
hemi2->next = NULL;
/* Store the origin point, radius and field strength for cylinder. */
V3Copy(&cyl->loc, pt1);
cyl->rad = radius;
cyl->field = field;
/*
* Pre-compute constants for the density eq. in standard form.
* Radii that are too small should be checked for during parse.
*/
cyl->rsq = cyl->rad * cyl->rad;
cyl->r2 = -( 2.0 * cyl->field) / cyl->rsq;
cyl->r4 = cyl->field / (cyl->rsq * cyl->rsq);
/* Get offset vector of cylinder from the given end point. */
V3Sub(&cyl->d, pt2, &cyl->loc);
/*
* Get cylinder length squared & length, while we're at it...
* Zero length offset vectors should be checked for during parse.
*/
cyl->lsq = V3Dot(&cyl->d, &cyl->d);
cyl->len = sqrt(cyl->lsq);
V3Copy(&cyl->dir, &cyl->d);
V3Normalize(&cyl->dir);
/* Get "d" coeffs. for planes at cylinder ends. */
cyl->d1 = -V3Dot(&cyl->dir, &cyl->loc);
cyl->d2 = -V3Dot(&cyl->dir, pt2);
/* Precompute location dot offset_vector constant. */
cyl->l_dot_d = V3Dot(&cyl->loc, &cyl->d);
cyl->type = BLOB_CYLINDER;
/* Set up the hemispheres for the ends of the cylinder. */
V3Copy(&hemi1->loc, &cyl->loc);
V3Sub(&hemi1->dir, pt2, &hemi1->loc);
V3Normalize(&hemi1->dir);
hemi1->rad = cyl->rad;
hemi1->rsq = cyl->rsq;
hemi1->field = cyl->field;
hemi1->r2 = cyl->r2;
hemi1->r4 = cyl->r4;
hemi1->type = BLOB_HEMISPHERE;
V3Copy(&hemi2->loc, pt2);
V3Sub(&hemi2->dir, &cyl->loc, &hemi2->loc);
V3Normalize(&hemi2->dir);
hemi2->rad = cyl->rad;
hemi2->rsq = cyl->rsq;
hemi2->field = cyl->field;
hemi2->r2 = cyl->r2;
hemi2->r4 = cyl->r4;
hemi2->type = BLOB_HEMISPHERE;
/*
* Finally, add the three new elements that make up the cylinder
* element to the blob.
*/
if (blob->elems != NULL)
{
Bloblet *lastbe;
for (lastbe = blob->elems; lastbe->next != NULL; lastbe = lastbe->next)
; /* Seek last element added to list. */
/* Append our new one. */
lastbe->next = cyl;
}
else
blob->elems = cyl;
return 1;
}
