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;
}
Point3 *NormalToFace(Face *f, Point3 *n)
{
Point3 t1,t2;
V3Sub(f->v[1],f->v[0],&t1);
V3Sub(f->v[2],f->v[0],&t2);
V3Cross(&t1,&t2,n);
return n;
}
Point3 *NormalToFace(Point3 *v, Face *f, Point3 *n)
{
Point3 t1,t2;
V3Sub(&(v[f->v[1]]),&(v[f->v[0]]),&t1);
V3Sub(&(v[f->v[2]]),&(v[f->v[0]]),&t2);
V3Cross(&t1,&t2,n);
return n;
}
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;
}
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;
}
PX_FORCE_INLINE PxcSIMDSpatial propagateDrivenImpulse(const PxcFsRow& row,
const PxcFsJointVectors& jv,
Vec3V& SZMinusQ,
const PxcSIMDSpatial& Z,
const Vec3V& Q)
{
typedef PxcArticulationFnsSimd<PxcArticulationFnsSimdBase> Fns;
SZMinusQ = V3Sub(V3Add(Z.angular, V3Cross(Z.linear,jv.jointOffset)), Q);
PxcSIMDSpatial result = Fns::translateForce(jv.parentOffset, Z - Fns::axisMultiply(row.DSI, SZMinusQ));
return result;
}
boolean PointBelongtoLine(Point3 *p, Line *l)
{
Point3 temp;
double t;
V3Sub(p,&(l->Lu),&temp);
t=temp.x/l->Lv.x;
if(fabs(l->Lv.y*t-temp.y) <= EPSILON) 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));
}
void PxcLtbComputeJv(Vec3V* jv, const PxcFsData& m, const PxcSIMDSpatial* velocity)
{
typedef PxcArticulationFnsSimd<PxcArticulationFnsSimdBase> Fns;
const PxcLtbRow* rows = getLtbRows(m);
const PxcFsRow* fsRows = getFsRows(m);
const PxcFsJointVectors* jointVectors = getJointVectors(m);
PX_UNUSED(rows);
PX_UNUSED(fsRows);
for(PxU32 i=1;i<m.linkCount;i++)
{
PxcSIMDSpatial pv = velocity[m.parent[i]], v = velocity[i];
Vec3V parentOffset = V3Add(jointVectors[i].jointOffset, jointVectors[i].parentOffset);
Vec3V k0v = V3Add(pv.linear, V3Cross(pv.angular, parentOffset)),
k1v = V3Add(v.linear, V3Cross(v.angular,jointVectors[i].jointOffset));
jv[i] = V3Sub(k0v, k1v);
}
}
extern int WriteCyl(FILE *fp, char *matName,
int id, Cyl3 *cyls)
{
int cylCnt = 0;
Cyl3 *cyl;
Vector3 dir;
char *material = matName;
#ifdef DEBUG
fprintf(stderr, "WriteCyl(%p, %p)\n", contblks, cyls);
#endif
if (cyls == NULL)
return 1;
for (cyl = cyls; cyl; cyl = cyl->next) {
if(matName == NULL) material = cyl->material;
if(cyl->erad == 0.0) { /* it's a point/sphere */
int sign;
sign = (cyl->srad > 0 ? 1 : -1);
if (sign > 0) {
fprintf(fp, "\n%s sphere %s.%d.%d\n",
material, material, id, cylCnt++);
} else {
fprintf(fp, "\n%s bubble %s.%d.%d\n",
material, material, id, cylCnt++);
}
fprintf(fp, "0\n0\n4");
fprintf(fp, "\t%.8g\t%.8g\t%.8g\t%.8g\n",
cyl->svert.x, cyl->svert.y, cyl->svert.z,
fabs(cyl->srad));
} else if (cyl->length != 0.0) { /* it's a cylinder or tube */
if(cyl->length >= 0.0) {
fprintf(fp, "\n%s cylinder %s.%d.%d\n",
material, material, id, cylCnt++);
} else {
fprintf(fp, "\n%s tube %s.%d.%d\n",
material, material, id, cylCnt++);
}
fprintf(fp, "0\n0\n7");
fprintf(fp, "\t%.8g %.8g %.8g\n", cyl->svert.x,
cyl->svert.y, cyl->svert.z);
fprintf(fp, "\t%.8g %.8g %.8g\n", cyl->evert.x,
cyl->evert.y, cyl->evert.z);
fprintf(fp, "\t%.8g\n", cyl->srad);
/* bottom cap */
fprintf(fp, "\n%s ring %s.%d.%d\n",
material, material, id, cylCnt++);
fprintf(fp, "0\n0\n8");
fprintf(fp, "\t%.8g %.8g %.8g\n",
cyl->svert.x, cyl->svert.y, cyl->svert.z);
if(cyl->length >= 0.0) {
(void)V3Normalize(V3Sub(&cyl->svert, &cyl->evert, &dir));
} else {
(void)V3Normalize(V3Sub(&cyl->evert, &cyl->svert, &dir));
}
fprintf(fp, "\t%.8g %.8g %.8g\n", dir.x, dir.y, dir.z);
fprintf(fp, "\t0 %.8g\n", cyl->srad);
/* top cap */
fprintf(fp, "\n%s ring %s.%d.%d\n",
material, material, id, cylCnt++);
fprintf(fp, "0\n0\n8");
fprintf(fp, "\t%.8g %.8g %.8g\n",
cyl->evert.x, cyl->evert.y, cyl->evert.z);
if(cyl->length >= 0.0) {
(void)V3Normalize(V3Sub(&cyl->evert, &cyl->svert, &dir));
} else {
(void)V3Normalize(V3Sub(&cyl->svert, &cyl->evert, &dir));
}
fprintf(fp, "\t%.8g %.8g %.8g\n", dir.x, dir.y, dir.z);
fprintf(fp, "\t0 %.8g\n", cyl->srad);
} else { /* it's a ring */
fprintf(fp, "\n%s ring %s.%d.%d\n",
material, material, id, cylCnt++);
fprintf(fp, "0\n0\n8");
fprintf(fp, "\t%.8g %.8g %.8g\n",
cyl->svert.x, cyl->svert.y, cyl->svert.z);
(void)V3Normalize(&cyl->normal);
fprintf(fp, "\t%.8g %.8g %.8g\n",
cyl->normal.x, cyl->normal.y, cyl->normal.z);
fprintf(fp, "\t0 %.8g\n", cyl->srad);
}
}
Cyl3FreeList(cyls);
return 1;
}
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
}
static MeshPod CreateTrefoil()
{
const int Slices = 256;
const int Stacks = 32;
const int VertexCount = Slices * Stacks;
const int IndexCount = VertexCount * 6;
MeshPod mesh;
glGenVertexArrays(1, &mesh.Vao);
glBindVertexArray(mesh.Vao);
// Create a buffer with interleaved positions and normals
if (true) {
Vertex verts[VertexCount];
Vertex* pVert = &verts[0];
float ds = 1.0f / Slices;
float dt = 1.0f / Stacks;
// The upper bounds in these loops are tweaked to reduce the
// chance of precision error causing an incorrect # of iterations.
for (float s = 0; s < 1 - ds / 2; s += ds) {
for (float t = 0; t < 1 - dt / 2; t += dt) {
const float E = 0.01f;
Vector3 p = EvaluateTrefoil(s, t);
Vector3 u = V3Sub(EvaluateTrefoil(s + E, t), p);
Vector3 v = V3Sub(EvaluateTrefoil(s, t + E), p);
Vector3 n = V3Normalize(V3Cross(u, v));
pVert->Position = p;
pVert->Normal = n;
++pVert;
}
}
pezCheck(pVert - &verts[0] == VertexCount, "Tessellation error.");
GLuint vbo;
GLsizeiptr size = sizeof(verts);
const GLvoid* data = &verts[0].Position.x;
GLenum usage = GL_STATIC_DRAW;
glGenBuffers(1, &vbo);
glBindBuffer(GL_ARRAY_BUFFER, vbo);
glBufferData(GL_ARRAY_BUFFER, size, data, usage);
}
// Create a buffer of 16-bit indices
if (true) {
GLushort inds[IndexCount];
GLushort* pIndex = &inds[0];
GLushort n = 0;
for (GLushort i = 0; i < Slices; i++) {
for (GLushort j = 0; j < Stacks; j++) {
*pIndex++ = (n + j + Stacks) % VertexCount;
*pIndex++ = n + (j + 1) % Stacks;
*pIndex++ = n + j;
*pIndex++ = (n + (j + 1) % Stacks + Stacks) % VertexCount;
*pIndex++ = (n + (j + 1) % Stacks) % VertexCount;
*pIndex++ = (n + j + Stacks) % VertexCount;
}
n += Stacks;
}
pezCheck(n == VertexCount, "Tessellation error.");
pezCheck(pIndex - &inds[0] == IndexCount, "Tessellation error.");
GLuint handle;
GLsizeiptr size = sizeof(inds);
const GLvoid* data = &inds[0];
GLenum usage = GL_STATIC_DRAW;
glGenBuffers(1, &handle);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, handle);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, size, data, usage);
}
mesh.VertexCount = VertexCount;
mesh.IndexCount = IndexCount;
glVertexAttribPointer(a("Position"), 3, GL_FLOAT, GL_FALSE, 24, 0);
glVertexAttribPointer(a("Normal"), 3, GL_FLOAT, GL_FALSE, 24, offset(12));
glEnableVertexAttribArray(a("Position"));
glEnableVertexAttribArray(a("Normal"));
pezCheck(OpenGLError);
return mesh;
}
bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
{
PX_UNUSED(renderOutput);
const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
PersistentContactManifold& manifold = cache.getManifold();
Ps::prefetchLine(shapeConvex.hullData);
PX_ASSERT(transform1.q.isSane());
PX_ASSERT(transform0.q.isSane());
const Vec3V zeroV = V3Zero();
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
const FloatV contactDist = FLoad(params.mContactDistance);
const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
const ConvexHullData* hullData =shapeConvex.hullData;
//Transfer A into the local space of B
const PsTransformV transf0 = loadTransformA(transform0);
const PsTransformV transf1 = loadTransformA(transform1);
const PsTransformV curRTrans(transf1.transformInv(transf0));
const PsMatTransformV aToB(curRTrans);
const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius);
const FloatV minMargin = FMin(convexMargin, capsuleMinMargin);
const PxU32 initialContacts = manifold.mNumContacts;
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f));
const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist);
//ML: after refreshContactPoints, we might lose some contacts
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
const FloatV zero = FZero();
FloatV penDep = zero;
PX_UNUSED(bLostContacts);
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
{
const bool idtScale = shapeConvex.scale.isIdentity();
manifold.setRelativeTransform(curRTrans);
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
convexHull.setMargin(zero);
//transform capsule(a) into the local space of convexHull(b)
CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
LocalConvex<CapsuleV> convexA(capsule);
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
bool doOverlapTest = false;
if(status == GJK_NON_INTERSECT)
{
return false;
}
else if(status == GJK_DEGENERATE)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength));
}
else
{
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
if(status == GJK_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
PX_ASSERT(status == EPA_CONTACT);
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
if(status == EPA_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
doOverlapTest = true;
}
}
if(initialContacts == 0 || bLostContacts || doOverlapTest)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
}
else
{
//This contact is either come from GJK or EPA
normal = transf1.rotate(normal);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
}
}
else if (manifold.getNumContacts() > 0)
{
normal = manifold.getWorldNormal(transf1);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
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;
}
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;
}
