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;
}
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;
}
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);
}
}
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;
}
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;
}
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
}
/*
* LoadSlopeInfo: Load data for sloped surface descriptions and generate
* runtime representation of the data describing the texture coordinates.
* Return pointer to new SlopeData record on success, NULL on error
*/
SlopeData *LoadSlopeInfo(file_node *f) {
int size;
long txt_angle;
long index;
SlopeData *new_slope;
Vector3D texture_orientation;
Vector3D v1,v2;
char junk[6];
// create a new slope record
size = sizeof(SlopeData);
new_slope = (SlopeData *) SafeMalloc(size);
memset(new_slope, 0, size);
// load coefficients of plane equation
if (CliMappedFileRead(f, &new_slope->plane.a, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->plane.b, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->plane.c, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->plane.d, 4) != 4) return (SlopeData *)NULL;
// dprintf("loaded equation a = %d, b = %d, c = %d, d = %d\n", new_slope->plane.a, new_slope->plane.b, new_slope->plane.c, new_slope->plane.d);
if (new_slope->plane.c == 0) {
debug(("Error: loaded plane equation equal to a vertical slope\n"));
// punt on error and stick in non crashing values (use assert instead?)
new_slope->plane.a = 0;
new_slope->plane.b = 0;
new_slope->plane.c = 1024;
new_slope->plane.d = 0;
}
// load x & y of texture origin
if (CliMappedFileRead(f, &new_slope->p0.x, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->p0.y, 4) != 4) return (SlopeData *)NULL;
// calculate z of texture origin from x, y, and plane equation
new_slope->p0.z = (-new_slope->plane.a*new_slope->p0.x - new_slope->plane.b*new_slope->p0.y - new_slope->plane.d)/new_slope->plane.c;
// load in texture angle - this is planar angle between x axis of texture & x axis of world
if (CliMappedFileRead(f, &txt_angle, 4) != 4) return (SlopeData *)NULL;
new_slope->texRot = txt_angle;
// convert angle to vector
texture_orientation.x = (long)Cos(txt_angle) >> 6;
texture_orientation.y = (long)Sin(txt_angle) >> 6;
texture_orientation.z = 0;
// generate other endpoints from plane normal, texture origin, and texture
// orientation which determine the orientation of the texture's u v space
// in the 3d world's x, y, z space
// cross normal with texture orientation to get vector perpendicular to texture
// orientation and normal = v axis direction
V3Cross((Vector3D *)&(new_slope->plane), &texture_orientation, &v2);
// scale to size of texture in world space
V3Scale(&v2, FINENESS);
// cross normal with v axis direction vector to get vector perpendicular to v axis
// and normal = u axis direction vector
V3Cross(&v2, (Vector3D *)&(new_slope->plane), &v1);
// scale to size of texture in world space
V3Scale(&v1, FINENESS);
// add vectors to origin to get endpoints
V3Add(&new_slope->p0, &v1, &new_slope->p1);
V3Add(&new_slope->p0, &v2, &new_slope->p2);
// set flags indicating properties of the slope
new_slope->flags = 0;
if (ABS(new_slope->plane.c) < DIRECTIONAL_THRESHOLD)
new_slope->flags |= SLF_DIRECTIONAL;
else if (new_slope->plane.c < 0) // ceiling, apply same lighting hack as regular ceilings (see doDrawLeaf)
new_slope->lightscale = FINENESS-(shade_amount>>1);
else
void PezRender()
{
#define Instances 7
Matrix4 Model[Instances];
Model[0] = M4MakeRotationY(Globals.Theta);
Model[1] = M4Mul(M4Mul(
M4MakeTranslation((Vector3){0, 0, 0.6}),
M4MakeScale(V3MakeFromScalar(0.25))),
M4MakeRotationX(Pi/2)
);
Model[2] = Model[3] = Model[4] = Model[1];
Model[1] = M4Mul(M4MakeRotationY(Globals.Theta), Model[1]);
Model[2] = M4Mul(M4MakeRotationY(Globals.Theta + Pi/2), Model[2]);
Model[3] = M4Mul(M4MakeRotationY(Globals.Theta - Pi/2), Model[3]);
Model[4] = M4Mul(M4MakeRotationY(Globals.Theta + Pi), Model[4]);
Model[5] = M4Mul(M4Mul(
M4MakeScale(V3MakeFromScalar(0.5)),
M4MakeTranslation((Vector3){0, 1.25, 0})),
M4MakeRotationY(-Globals.Theta)
);
Model[6] = M4Mul(M4Mul(
M4MakeScale(V3MakeFromScalar(0.5)),
M4MakeTranslation((Vector3){0, -1.25, 0})),
M4MakeRotationY(-Globals.Theta)
);
Vector3 LightPosition = {0.5, 0.25, 1.0}; // world space
Vector3 EyePosition = {0, 0, 1}; // world space
Matrix4 MVP[Instances];
Vector3 Lhat[Instances];
Vector3 Hhat[Instances];
for (int i = 0; i < Instances; i++) {
Matrix4 mv = M4Mul(Globals.View, Model[i]);
MVP[i] = M4Mul(Globals.Projection, mv);
Matrix3 m = M3Transpose(M4GetUpper3x3(Model[i]));
Lhat[i] = M3MulV3(m, V3Normalize(LightPosition)); // object space
Vector3 Eye = M3MulV3(m, V3Normalize(EyePosition)); // object space
Hhat[i] = V3Normalize(V3Add(Lhat[i], Eye));
}
int instanceCount = Instances;
MeshPod* mesh = &Globals.Cylinder;
glBindFramebuffer(GL_FRAMEBUFFER, Globals.FboHandle);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glUseProgram(Globals.LitProgram);
glUniform3f(u("SpecularMaterial"), 0.4, 0.4, 0.4);
glUniform4f(u("FrontMaterial"), 0, 0, 1, 1);
glUniform4f(u("BackMaterial"), 0.5, 0.5, 0, 1);
glUniform3fv(u("Hhat"), Instances, &Hhat[0].x);
glUniform3fv(u("Lhat"), Instances, &Lhat[0].x);
glUniformMatrix4fv(u("ModelviewProjection"), Instances, 0, (float*) &MVP[0]);
glBindVertexArray(mesh->FillVao);
glDrawElementsInstanced(GL_TRIANGLES, mesh->FillIndexCount, GL_UNSIGNED_SHORT, 0, instanceCount);
glUseProgram(Globals.SimpleProgram);
glUniform4f(u("Color"), 0, 0, 0, 1);
glUniformMatrix4fv(u("ModelviewProjection"), Instances, 0, (float*) &MVP[0]);
glDepthMask(GL_FALSE);
glBindVertexArray(mesh->LineVao);
glDrawElementsInstanced(GL_LINES, mesh->LineIndexCount, GL_UNSIGNED_SHORT, 0, instanceCount);
glDepthMask(GL_TRUE);
glDisable(GL_DEPTH_TEST);
glBindFramebuffer(GL_FRAMEBUFFER, 0);
glUseProgram(Globals.QuadProgram);
glBindTexture(GL_TEXTURE_2D, Globals.FboTexture);
glBindVertexArray(Globals.Grid.FillVao);
glDrawElements(GL_TRIANGLES, Globals.Grid.FillIndexCount, GL_UNSIGNED_SHORT, 0);
if (1) {
glUseProgram(Globals.GridProgram);
glBindVertexArray(Globals.Grid.LineVao);
glDrawElements(GL_LINES, Globals.Grid.LineIndexCount, GL_UNSIGNED_SHORT, 0);
}
glBindTexture(GL_TEXTURE_2D, 0);
}
