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