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; }