void setupFrame(void) { static Matrix world; static Matrix view; SVGA3dTextureState *ts; SVGA3dRenderState *rs; Matrix_Copy(view, gIdentityMatrix); Matrix_Translate(view, 0, 0, 3); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_VIEW, view); Matrix_Copy(world, gIdentityMatrix); Matrix_RotateX(world, -60.0 * PI_OVER_180); Matrix_RotateY(world, gFPS.frame * 0.01f); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_WORLD, world); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_PROJECTION, perspectiveMat); SVGA3D_BeginSetRenderState(CID, &rs, 4); { rs[0].state = SVGA3D_RS_BLENDENABLE; rs[0].uintValue = FALSE; rs[1].state = SVGA3D_RS_ZENABLE; rs[1].uintValue = TRUE; rs[2].state = SVGA3D_RS_ZWRITEENABLE; rs[2].uintValue = TRUE; rs[3].state = SVGA3D_RS_ZFUNC; rs[3].uintValue = SVGA3D_CMP_LESS; } SVGA_FIFOCommitAll(); SVGA3D_BeginSetTextureState(CID, &ts, 4); { ts[0].stage = 0; ts[0].name = SVGA3D_TS_BIND_TEXTURE; ts[0].value = SVGA3D_INVALID_ID; ts[1].stage = 0; ts[1].name = SVGA3D_TS_COLOROP; ts[1].value = SVGA3D_TC_SELECTARG1; ts[2].stage = 0; ts[2].name = SVGA3D_TS_COLORARG1; ts[2].value = SVGA3D_TA_DIFFUSE; ts[3].stage = 0; ts[3].name = SVGA3D_TS_ALPHAARG1; ts[3].value = SVGA3D_TA_DIFFUSE; } SVGA_FIFOCommitAll(); }
/** extracts the equalities holding on the parameters only, try to introduce them back and compare the two polyhedra. Reads a polyhedron and a context. */ int test_Polyhedron_Remove_parm_eqs(Matrix * A, Matrix * B) { int isOk = 1; Matrix * M, *C; Polyhedron *Pm, *Pc, *Pcp, *Peqs, *Pint, *Pint1; unsigned int * elimParms; printf("----- test_Polyhedron_Remove_parm_eqs() -----\n"); M = Matrix_Copy(A); C = Matrix_Copy(B); /* compute the combined polyhedron */ Pm = Constraints2Polyhedron(M, maxRays); Pc = Constraints2Polyhedron(C, maxRays); Pcp = align_context(Pc, Pm->Dimension, maxRays); Polyhedron_Free(Pc); Pint1 = DomainIntersection(Pm, Pcp, maxRays); Polyhedron_Free(Pm); Polyhedron_Free(Pcp); Matrix_Free(M); Matrix_Free(C); M = Matrix_Copy(A); C = Matrix_Copy(B); /* extract the parm-equalities, expressed in the combined space */ Pm = Constraints2Polyhedron(M, maxRays); Pc = Constraints2Polyhedron(C, maxRays); Matrix_Free(M); Matrix_Free(C); Peqs = Polyhedron_Remove_parm_eqs(&Pm, &Pc, 1, &elimParms, 200); /* compute the supposedly-same polyhedron, using the extracted equalities */ Pcp = align_context(Pc, Pm->Dimension, maxRays); Polyhedron_Free(Pc); Pc = DomainIntersection(Pm, Pcp, maxRays); Polyhedron_Free(Pm); Polyhedron_Free(Pcp); Pint = DomainIntersection(Pc, Peqs, maxRays); Polyhedron_Free(Pc); Polyhedron_Free(Peqs); /* test their equality */ if (!PolyhedronIncludes(Pint, Pint1)) { isOk = 0; } else { if (!PolyhedronIncludes(Pint1, Pint)) { isOk = 0; } } Polyhedron_Free(Pint1); Polyhedron_Free(Pint); return isOk; } /* test_Polyhedron_remove_parm_eqs() */
void LU_Refactorize(PT_Basis pB) { char L = 'L'; /* lower triangular */ char D = 'U'; /* unit triangular matrix (diagonals are ones) */ ptrdiff_t info, incx=1, incp; /* Matrix_Print_row(pB->pLX); */ /* Matrix_Print_utril_row(pB->pUX); */ /* factorize using lapack */ dgetrf(&(Matrix_Rows(pB->pF)), &(Matrix_Rows(pB->pF)), pMAT(pB->pF), &((pB->pF)->rows_alloc), pB->p, &info); /* store upper triangular matrix (including the diagonal to Ut), i.e. copy Ut <- F */ /* lapack ignores remaining elements below diagonal when computing triangular solutions */ Matrix_Copy(pB->pF, pB->pUt, pB->w); /* transform upper part of F (i.e. Ut) to triangular row major matrix UX*/ /* UX <- F */ Matrix_Full2RowTriangular(pB->pF, pB->pUX, pB->r); /* invert lower triangular part */ dtrtri( &L, &D, &(Matrix_Rows(pB->pF)), pMAT(pB->pF), &((pB->pF)->rows_alloc), &info); /* set strictly upper triangular parts to zeros because L is a full matrix * and we need zeros to compute proper permutation inv(L)*P */ Matrix_Uzeros(pB->pF); /* transpose matrix because dlaswp operates rowwise and we need columnwise */ /* LX <- F' */ Matrix_Transpose(pB->pF, pB->pLX, pB->r); /* interchange columns according to pivots in pB->p and write to LX*/ incp = -1; /* go backwards */ dlaswp( &(Matrix_Rows(pB->pLX)), pMAT(pB->pLX), &((pB->pLX)->rows_alloc), &incx, &(Matrix_Rows(pB->pLX)) , pB->p, &incp); /* Matrix_Print_col(pB->pX); */ /* Matrix_Print_row(pB->pLX); */ /* Matrix_Print_col(pB->pUt); */ /* Matrix_Print_utril_row(pB->pUX); */ /* matrix F after solution is factored in [L\U], we want the original format for the next call to dgesv, thus create a copy F <- X */ Matrix_Copy(pB->pX, pB->pF, pB->w); }
/* * CG_TestEntities * * If cg_testEntities is set, create 32 player models */ static void CG_TestEntities( void ) { int i, j; float f, r; entity_t ent; memset( &ent, 0, sizeof( ent ) ); trap_R_ClearScene(); for( i = 0; i < 100; i++ ) { r = 64 * ( ( i%4 ) - 1.5 ); f = 64 * ( i/4 ) + 128; for( j = 0; j < 3; j++ ) ent.origin[j] = ent.lightingOrigin[j] = cg.view.origin[j] + cg.view.axis[FORWARD][j]*f + cg.view.axis[RIGHT][j]*r; Matrix_Copy( cg.autorotateAxis, ent.axis ); ent.scale = 1.0f; ent.rtype = RT_MODEL; // skelmod splitmodels ent.model = cgs.basePModelInfo->model; if( cgs.baseSkin ) ent.customSkin = cgs.baseSkin; else ent.customSkin = NULL; CG_AddEntityToScene( &ent ); // skelmod } }
/* * Given Lattice 'Lat' and a Polyhderon 'Poly', allocate space, and return * the Z-polyhderon corresponding to the image of the polyhderon 'Poly' by the * lattice 'Lat'. If the input lattice 'Lat' is not integeral, it integralises * it, i.e. the lattice of the Z-polyhderon returned is integeral. */ ZPolyhedron *ZPolyhedron_Alloc(Lattice *Lat, Polyhedron *Poly) { ZPolyhedron *A; POL_ENSURE_FACETS(Poly); POL_ENSURE_VERTICES(Poly); if(Lat->NbRows != Poly->Dimension+1) { fprintf(stderr,"\nInZPolyAlloc - The Lattice and the Polyhedron"); fprintf(stderr," are not compatible to form a ZPolyhedra\n"); return NULL; } if((!(isEmptyLattice(Lat))) && (!isfulldim (Lat))) { fprintf(stderr,"\nZPolAlloc: Lattice not Full Dimensional\n"); return NULL; } A = (ZPolyhedron *)malloc(sizeof(ZPolyhedron)); if (!A) { fprintf(stderr,"ZPolAlloc : Out of Memory\n"); return NULL; } A->next = NULL; A->P = Domain_Copy(Poly); A->Lat = Matrix_Copy(Lat); if(IsLattice(Lat) == False) { ZPolyhedron *Res; Res = IntegraliseLattice (A); ZPolyhedron_Free (A); return Res; } return A; } /* ZPolyhedron_Alloc */
/* * CG_PModel_SpawnTeleportEffect */ void CG_PModel_SpawnTeleportEffect( centity_t *cent ) { // the thing is, we must have a built skeleton, so we // can run all bones and spawn a polygon at their origin int i, j; cgs_skeleton_t *skel; orientation_t orient, ref; float radius = 5; lentity_t *le; vec3_t vec, teleportOrigin; skel = CG_SkeletonForModel( cent->ent.model ); if( !skel || !cent->ent.boneposes ) return; for( j = LOCALEFFECT_EV_PLAYER_TELEPORT_IN; j <= LOCALEFFECT_EV_PLAYER_TELEPORT_OUT; j++ ) { if( cent->localEffects[j] ) { cent->localEffects[j] = 0; if( j == LOCALEFFECT_EV_PLAYER_TELEPORT_OUT ) VectorCopy( cent->teleportedFrom, teleportOrigin ); else VectorCopy( cent->ent.origin, teleportOrigin ); for( i = 0; i < skel->numBones; i++ ) { DualQuat_ToMatrixAndVector( cent->ent.boneposes[i].dualquat, orient.axis, orient.origin ); VectorCopy( vec3_origin, ref.origin ); Matrix_Copy( axis_identity, ref.axis ); CG_MoveToTag( ref.origin, ref.axis, teleportOrigin, cent->ent.axis, orient.origin, orient.axis ); VectorSet( vec, 0.1f, 0.1f, 0.1f ); // spawn a sprite at each bone le = CG_AllocSprite( LE_SCALE_ALPHA_FADE, ref.origin, radius, 15 + crandom()*5, 1, 1, 1, 0.5f, 0, 0, 0, 0, CG_MediaShader( cgs.media.shaderTeleporterSmokePuff ) ); VectorSet( le->velocity, -vec[0] * 5 + crandom()*5, -vec[1] * 5 + crandom()*5, -vec[2] * 5 + crandom()*5 + 3 ); le->ent.rotation = rand() % 360; // CG_ParticleEffect( ref.origin, ref.axis[2], 0.9f, 0.9f, 0.9f, 2 ); } } } }
/* creates column major upper triangular matrix F from U */ void Matrix_Triu(PT_Matrix pF, PT_Matrix pU, double *w) { ptrdiff_t r, c; /* creates a copy F <- U */ Matrix_Copy(pU, pF, w); /* put upper diagonal elements to proper position in matrix F */ for (r=0; r<pU->rows; r++) { for (c=0; c<pU->cols; c++) { if (c<r) { /* lower triangular parts of pF are zeros */ pF->A[r+c*pF->rows_alloc] = 0.0; } else { /* access upper triangular elements from matrix pU */ pF->A[r+c*pF->rows_alloc] = pU->A[pU->rows_alloc*r+c-(r+1)*r/2]; } } } }
/* ================ CG_DrawModel ================ */ static void CG_DrawModel( int x, int y, int align, int w, int h, struct model_s *model, struct shader_s *shader, vec3_t origin, vec3_t angles, qboolean outline ) { refdef_t refdef; entity_t entity; if( !model ) return; x = CG_HorizontalAlignForWidth( x, align, w ); y = CG_VerticalAlignForHeight( y, align, h ); memset( &refdef, 0, sizeof( refdef ) ); refdef.x = x; refdef.y = y; refdef.width = w; refdef.height = h; refdef.fov_x = 30; refdef.fov_y = CalcFov( refdef.fov_x, w, h ); refdef.time = cg.time; refdef.rdflags = RDF_NOWORLDMODEL; Matrix_Copy( axis_identity, refdef.viewaxis ); memset( &entity, 0, sizeof( entity ) ); entity.model = model; entity.customShader = shader; entity.scale = 1.0f; entity.renderfx = RF_FULLBRIGHT | RF_NOSHADOW | RF_FORCENOLOD; VectorCopy( origin, entity.origin ); VectorCopy( entity.origin, entity.origin2 ); AnglesToAxis( angles, entity.axis ); if( outline ) { entity.outlineHeight = DEFAULT_OUTLINE_HEIGHT; Vector4Set( entity.outlineRGBA, 0, 0, 0, 255 ); } trap_R_ClearScene(); CG_SetBoneposesForTemporaryEntity( &entity ); CG_AddEntityToScene( &entity ); trap_R_RenderScene( &refdef ); }
/** * Given a matrix that defines a full-dimensional affine lattice, returns the * affine sub-lattice spanned in the k first dimensions. * Useful for instance when you only look for the parameters' validity lattice. * @param lat the original full-dimensional lattice * @param subLat the sublattice */ void Lattice_extractSubLattice(Matrix * lat, unsigned int k, Matrix ** subLat) { Matrix * H, *Q, *U, *linLat = NULL; unsigned int i; dbgStart(Lattice_extractSubLattice); /* if the dimension is already good, just copy the initial lattice */ if (k==lat->NbRows-1) { if (*subLat==NULL) { (*subLat) = Matrix_Copy(lat); } else { Matrix_copySubMatrix(lat, 0, 0, lat->NbRows, lat->NbColumns, (*subLat), 0, 0); } return; } assert(k<lat->NbRows-1); /* 1- Make the linear part of the lattice triangular to eliminate terms from other dimensions */ Matrix_subMatrix(lat, 0, 0, lat->NbRows, lat->NbColumns-1, &linLat); /* OPT: any integer column-vector elimination is ok indeed. */ /* OPT: could test if the lattice is already in triangular form. */ left_hermite(linLat, &H, &Q, &U); if (dbgCompParmMore) { show_matrix(H); } Matrix_Free(Q); Matrix_Free(U); Matrix_Free(linLat); /* if not allocated yet, allocate it */ if (*subLat==NULL) { (*subLat) = Matrix_Alloc(k+1, k+1); } Matrix_copySubMatrix(H, 0, 0, k, k, (*subLat), 0, 0); Matrix_Free(H); Matrix_copySubMatrix(lat, 0, lat->NbColumns-1, k, 1, (*subLat), 0, k); for (i=0; i<k; i++) { value_set_si((*subLat)->p[k][i], 0); } value_set_si((*subLat)->p[k][k], 1); dbgEnd(Lattice_extractSubLattice); } /* Lattice_extractSubLattice */
/* headingKalman * * Implements a 1-dimensional, 1st order Kalman Filter * * That is, it deals with heading and heading rate (h and h') but no other * state variables. The state equations are: * * X = A X^ * h = h + h'dt --> | h | = | 1 dt | | h | * h' = h' | h' | | 0 1 | | h' | * * Kalman Filtering is not that hard. If it's hard you haven't found the right * teacher. Try taking CS373 from Udacity.com * * This notation is Octave (Matlab) syntax and is based on the Bishop-Welch * paper and references the equation numbers in that paper. * http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html * * returns : current heading estimate */ float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro) { A[1] = dt; /* Initialize, first time thru x = H*z0 */ //fprintf(stdout, "gyro? %c gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N'); // Depending on what sensor measurements we've gotten, // switch between observer (H) matrices and measurement noise (R) matrices // TODO 3 incorporate HDOP or sat count in R if (gps) { H[0] = 1.0; z[0] = Hgps; } else { H[0] = 0; z[0] = 0; } if (gyro) { H[3] = 1.0; z[1] = dHgyro; } else { H[3] = 0; z[1] = 0; } //Matrix_print(2,2, A, "1. A"); //Matrix_print(2,2, P, " P"); //Matrix_print(2,1, x, " x"); //Matrix_print(2,1, K, " K"); //Matrix_print(2,2, H, "2. H"); //Matrix_print(2,1, z, " z"); /********************************************************************** * Predict % * In this step we "move" our state estimate according to the equation * * x = A*x; // Eq 1.9 ***********************************************************************/ float xp[2]; Matrix_Multiply(2,2,1, xp, A, x); //Matrix_print(2,1, xp, "3. xp"); /********************************************************************** * We also have to "move" our uncertainty and add noise. Whenever we move, * we lose certainty because of system noise. * * P = A*P*A' + Q; // Eq 1.10 ***********************************************************************/ float At[4]; Matrix_Transpose(2,2, At, A); float AP[4]; Matrix_Multiply(2,2,2, AP, A, P); float APAt[4]; Matrix_Multiply(2,2,2, APAt, AP, At); Matrix_Add(2,2, P, APAt, Q); //Matrix_print(2,2, P, "4. P"); /********************************************************************** * Measurement aka Correct * First, we have to figure out the Kalman Gain which is basically how * much we trust the sensor measurement versus our prediction. * * K = P*H'*inv(H*P*H' + R); // Eq 1.11 ***********************************************************************/ float Ht[4]; //Matrix_print(2,2, H, "5. H"); Matrix_Transpose(2,2, Ht, H); //Matrix_print(2,2, Ht, "5. Ht"); float HP[2]; //Matrix_print(2,2, P, "5. P"); Matrix_Multiply(2,2,2, HP, H, P); //Matrix_print(2,2, HP, "5. HP"); float HPHt[4]; Matrix_Multiply(2,2,2, HPHt, HP, Ht); //Matrix_print(2,2, HPHt, "5. HPHt"); float HPHtR[4]; //Matrix_print(2,2, R, "5. R"); Matrix_Add(2,2, HPHtR, HPHt, R); //Matrix_print(2,2, HPHtR, "5. HPHtR"); Matrix_Inverse(2, HPHtR); //Matrix_print(2,2, HPHtR, "5. HPHtR"); float PHt[2]; //Matrix_print(2,2, P, "5. P"); //Matrix_print(2,2, Ht, "5. Ht"); Matrix_Multiply(2,2,2, PHt, P, Ht); //Matrix_print(2,2, PHt, "5. PHt"); Matrix_Multiply(2,2,2, K, PHt, HPHtR); //Matrix_print(2,2, K, "5. K"); /********************************************************************** * Then we determine the discrepancy between prediction and measurement * with the "Innovation" or Residual: z-H*x, multiply that by the * Kalman gain to correct the estimate towards the prediction a little * at a time. * * x = x + K*(z-H*x); // Eq 1.12 ***********************************************************************/ float Hx[2]; Matrix_Multiply(2,2,1, Hx, H, xp); //Matrix_print(2,2, H, "6. H"); //Matrix_print(2,1, x, "6. x"); //Matrix_print(2,1, Hx, "6. Hx"); float zHx[2]; Matrix_Subtract(2,1, zHx, z, Hx); zHx[0] = clamp180(zHx[0]); //Matrix_print(2,1, z, "6. z"); //Matrix_print(2,1, zHx, "6. zHx"); float KzHx[2]; Matrix_Multiply(2,2,1, KzHx, K, zHx); //Matrix_print(2,2, K, "6. K"); //Matrix_print(2,1, KzHx, "6. KzHx"); Matrix_Add(2,1, x, xp, KzHx); x[0] = clamp360(x[0]); // Clamp to 0-360 range //Matrix_print(2,1, x, "6. x"); /********************************************************************** * We also have to adjust the certainty. With a new measurement, the * estimate certainty always increases. * * P = (I-K*H)*P; // Eq 1.13 ***********************************************************************/ float KH[4]; //Matrix_print(2,2, K, "7. K"); Matrix_Multiply(2,2,2, KH, K, H); //Matrix_print(2,2, KH, "7. KH"); float IKH[4]; Matrix_Subtract(2,2, IKH, I, KH); //Matrix_print(2,2, IKH, "7. IKH"); float P2[4]; Matrix_Multiply(2,2,2, P2, IKH, P); Matrix_Copy(2, 2, P, P2); //Matrix_print(2,2, P, "7. P"); return x[0]; }
void CG_DrawTestBox( vec3_t origin, vec3_t mins, vec3_t maxs, vec3_t angles ) { vec3_t start, end, vec; float linewidth = 6; vec3_t localAxis[3]; #if 1 vec3_t ax[3]; AnglesToAxis( angles, ax ); Matrix_Transpose( ax, localAxis ); #else Matrix_Copy( axis_identity, localAxis ); if( angles[YAW] ) Matrix_Rotate( localAxis, -angles[YAW], 0, 0, 1 ); if( angles[PITCH] ) Matrix_Rotate( localAxis, -angles[PITCH], 0, 1, 0 ); if( angles[ROLL] ) Matrix_Rotate( localAxis, -angles[ROLL], 1, 0, 0 ); #endif //horizontal projection start[0] = mins[0]; start[1] = mins[1]; start[2] = mins[2]; end[0] = mins[0]; end[1] = mins[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = mins[0]; start[1] = maxs[1]; start[2] = mins[2]; end[0] = mins[0]; end[1] = maxs[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = maxs[0]; start[1] = mins[1]; start[2] = mins[2]; end[0] = maxs[0]; end[1] = mins[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = maxs[0]; start[1] = maxs[1]; start[2] = mins[2]; end[0] = maxs[0]; end[1] = maxs[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); //x projection start[0] = mins[0]; start[1] = mins[1]; start[2] = mins[2]; end[0] = maxs[0]; end[1] = mins[1]; end[2] = mins[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = mins[0]; start[1] = maxs[1]; start[2] = maxs[2]; end[0] = maxs[0]; end[1] = maxs[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = mins[0]; start[1] = maxs[1]; start[2] = mins[2]; end[0] = maxs[0]; end[1] = maxs[1]; end[2] = mins[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = mins[0]; start[1] = mins[1]; start[2] = maxs[2]; end[0] = maxs[0]; end[1] = mins[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); //z projection start[0] = mins[0]; start[1] = mins[1]; start[2] = mins[2]; end[0] = mins[0]; end[1] = maxs[1]; end[2] = mins[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = maxs[0]; start[1] = mins[1]; start[2] = maxs[2]; end[0] = maxs[0]; end[1] = maxs[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = maxs[0]; start[1] = mins[1]; start[2] = mins[2]; end[0] = maxs[0]; end[1] = maxs[1]; end[2] = mins[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); start[0] = mins[0]; start[1] = mins[1]; start[2] = maxs[2]; end[0] = mins[0]; end[1] = maxs[1]; end[2] = maxs[2]; // convert to local axis space VectorCopy( start, vec ); Matrix_TransformVector( localAxis, vec, start ); VectorCopy( end, vec ); Matrix_TransformVector( localAxis, vec, end ); VectorAdd( origin, start, start ); VectorAdd( origin, end, end ); CG_QuickPolyBeam( start, end, linewidth, NULL ); }
void renderCube(float x, float y, Bool useShaders, Bool useHalf) { SVGA3dTextureState *ts; SVGA3dRenderState *rs; SVGA3dVertexDecl *decls; SVGA3dPrimitiveRange *ranges; static Matrix view; Matrix_Copy(view, gIdentityMatrix); Matrix_RotateX(view, 30.0 * M_PI / 180.0); Matrix_RotateY(view, gFPS.frame * 0.01f); Matrix_Translate(view, x, y, 15); if (useShaders) { SVGA3D_SetShader(CID, SVGA3D_SHADERTYPE_VS, MY_VSHADER_ID); SVGA3D_SetShader(CID, SVGA3D_SHADERTYPE_PS, MY_PSHADER_ID); SVGA3DUtil_SetShaderConstMatrix(CID, CONST_MAT_PROJ, SVGA3D_SHADERTYPE_VS, perspectiveMat); SVGA3DUtil_SetShaderConstMatrix(CID, CONST_MAT_VIEW, SVGA3D_SHADERTYPE_VS, view); } else { SVGA3D_SetShader(CID, SVGA3D_SHADERTYPE_VS, SVGA3D_INVALID_ID); SVGA3D_SetShader(CID, SVGA3D_SHADERTYPE_PS, SVGA3D_INVALID_ID); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_VIEW, view); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_WORLD, gIdentityMatrix); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_PROJECTION, perspectiveMat); } SVGA3D_BeginSetRenderState(CID, &rs, 4); { rs[0].state = SVGA3D_RS_BLENDENABLE; rs[0].uintValue = FALSE; rs[1].state = SVGA3D_RS_ZENABLE; rs[1].uintValue = TRUE; rs[2].state = SVGA3D_RS_ZWRITEENABLE; rs[2].uintValue = TRUE; rs[3].state = SVGA3D_RS_ZFUNC; rs[3].uintValue = SVGA3D_CMP_LESS; } SVGA_FIFOCommitAll(); SVGA3D_BeginSetTextureState(CID, &ts, 4); { ts[0].stage = 0; ts[0].name = SVGA3D_TS_BIND_TEXTURE; ts[0].value = SVGA3D_INVALID_ID; ts[1].stage = 0; ts[1].name = SVGA3D_TS_COLOROP; ts[1].value = SVGA3D_TC_SELECTARG1; ts[2].stage = 0; ts[2].name = SVGA3D_TS_COLORARG1; ts[2].value = SVGA3D_TA_DIFFUSE; ts[3].stage = 0; ts[3].name = SVGA3D_TS_ALPHAARG1; ts[3].value = SVGA3D_TA_DIFFUSE; } SVGA_FIFOCommitAll(); SVGA3D_BeginDrawPrimitives(CID, &decls, 2, &ranges, 1); { decls[0].identity.usage = SVGA3D_DECLUSAGE_POSITION; decls[0].array.surfaceId = vertexSid; decls[0].array.stride = sizeof(MyVertex); if (useHalf) { decls[0].identity.type = SVGA3D_DECLTYPE_FLOAT16_4; decls[0].array.offset = offsetof(MyVertex, position16); } else { decls[0].identity.type = SVGA3D_DECLTYPE_FLOAT3; decls[0].array.offset = offsetof(MyVertex, position32); } decls[1].identity.type = SVGA3D_DECLTYPE_D3DCOLOR; decls[1].identity.usage = SVGA3D_DECLUSAGE_COLOR; decls[1].array.surfaceId = vertexSid; decls[1].array.stride = sizeof(MyVertex); decls[1].array.offset = offsetof(MyVertex, color); ranges[0].primType = SVGA3D_PRIMITIVE_TRIANGLELIST; ranges[0].primitiveCount = numTriangles; ranges[0].indexArray.surfaceId = indexSid; ranges[0].indexArray.stride = sizeof(uint16); ranges[0].indexWidth = sizeof(uint16); } SVGA_FIFOCommitAll(); SVGA3D_SetShader(CID, SVGA3D_SHADERTYPE_VS, SVGA3D_INVALID_ID); SVGA3D_SetShader(CID, SVGA3D_SHADERTYPE_PS, SVGA3D_INVALID_ID); }
/* * CG_SkeletalPoseGetAttachment * Get the tag from the interpolated and transformed pose */ qboolean CG_SkeletalPoseGetAttachment( orientation_t *orient, cgs_skeleton_t *skel, bonepose_t *boneposes, char *bonename ) { int i; quat_t quat; cgs_bone_t *bone; bonepose_t *bonepose; cg_tagmask_t *tagmask; if( !boneposes || !skel ) { CG_Printf( "CG_SkeletalPoseLerpAttachment: Wrong model or boneposes %s\n", bonename ); return qfalse; } tagmask = CG_TagMask( bonename, skel ); // find the appropriate attachment bone if( tagmask ) { bone = skel->bones; for( i = 0; i < skel->numBones; i++, bone++ ) { if( !Q_stricmp( bone->name, tagmask->bonename ) ) break; } } else { bone = skel->bones; for( i = 0; i < skel->numBones; i++, bone++ ) { if( !Q_stricmp( bone->name, bonename ) ) break; } } if( i == skel->numBones ) { CG_Printf( "CG_SkeletalPoseLerpAttachment: no such bone %s\n", bonename ); return qfalse; } // get the desired bone bonepose = boneposes + i; // copy the inverted bone into the tag Quat_Inverse( &bonepose->dualquat[0], quat ); // inverse the tag direction Quat_Matrix( quat, orient->axis ); DualQuat_GetVector( bonepose->dualquat, orient->origin ); // normalize each axis for( i = 0; i < 3; i++ ) VectorNormalizeFast( orient->axis[i] ); // do the offseting if having a tagmask if( tagmask ) { // we want to place a rotated model over this tag, not to rotate the tag, // because all rotations would move. So we create a new orientation for the // model and we position the new orientation in tag space if( tagmask->rotate[YAW] || tagmask->rotate[PITCH] || tagmask->rotate[ROLL] ) { orientation_t modOrient, newOrient; VectorCopy( tagmask->offset, modOrient.origin ); AnglesToAxis( tagmask->rotate, modOrient.axis ); VectorCopy( vec3_origin, newOrient.origin ); Matrix_Identity( newOrient.axis ); CG_MoveToTag( newOrient.origin, newOrient.axis, orient->origin, orient->axis, modOrient.origin, modOrient.axis ); Matrix_Copy( newOrient.axis, orient->axis ); VectorCopy( newOrient.origin, orient->origin ); } else { // offset for( i = 0; i < 3; i++ ) { if( tagmask->offset[i] ) VectorMA( orient->origin, tagmask->offset[i], orient->axis[i], orient->origin ); } } } return qtrue; }
/** * Tests Constraints_fullDimensionize by comparing the Ehrhart polynomials * @param A the input set of constraints * @param B the corresponding context * @param the number of samples to generate for the test * @return 1 if the Ehrhart polynomial had the same value for the * full-dimensional and non-full-dimensional sets of constraints, for their * corresponding sample parameters values. */ int test_Constraints_fullDimensionize(Matrix * A, Matrix * B, unsigned int nbSamples) { Matrix * Eqs= NULL, *ParmEqs=NULL, *VL=NULL; unsigned int * elimVars=NULL, * elimParms=NULL; Matrix * sample, * smallerSample=NULL; Matrix * transfSample=NULL; Matrix * parmVL=NULL; unsigned int i, j, r, nbOrigParms, nbParms; Value div, mod, *origVal=NULL, *fullVal=NULL; Matrix * VLInv; Polyhedron * P, *PC; Matrix * M, *C; Enumeration * origEP, * fullEP=NULL; const char **fullNames = NULL; int isOk = 1; /* holds the result */ /* compute the origial Ehrhart polynomial */ M = Matrix_Copy(A); C = Matrix_Copy(B); P = Constraints2Polyhedron(M, maxRays); PC = Constraints2Polyhedron(C, maxRays); origEP = Polyhedron_Enumerate(P, PC, maxRays, origNames); Matrix_Free(M); Matrix_Free(C); Polyhedron_Free(P); Polyhedron_Free(PC); /* compute the full-dimensional polyhedron corresponding to A and its Ehrhart polynomial */ M = Matrix_Copy(A); C = Matrix_Copy(B); nbOrigParms = B->NbColumns-2; Constraints_fullDimensionize(&M, &C, &VL, &Eqs, &ParmEqs, &elimVars, &elimParms, maxRays); if ((Eqs->NbRows==0) && (ParmEqs->NbRows==0)) { Matrix_Free(M); Matrix_Free(C); Matrix_Free(Eqs); Matrix_Free(ParmEqs); free(elimVars); free(elimParms); return 1; } nbParms = C->NbColumns-2; P = Constraints2Polyhedron(M, maxRays); PC = Constraints2Polyhedron(C, maxRays); namesWithoutElim(origNames, nbOrigParms, elimParms, &fullNames); fullEP = Polyhedron_Enumerate(P, PC, maxRays, fullNames); Matrix_Free(M); Matrix_Free(C); Polyhedron_Free(P); Polyhedron_Free(PC); /* make a set of sample parameter values and compare the corresponding Ehrhart polnomials */ sample = Matrix_Alloc(1,nbOrigParms); transfSample = Matrix_Alloc(1, nbParms); Lattice_extractSubLattice(VL, nbParms, &parmVL); VLInv = Matrix_Alloc(parmVL->NbRows, parmVL->NbRows+1); MatInverse(parmVL, VLInv); if (dbg) { show_matrix(parmVL); show_matrix(VLInv); } srand(nbSamples); value_init(mod); value_init(div); for (i = 0; i< nbSamples; i++) { /* create a random sample */ for (j=0; j< nbOrigParms; j++) { value_set_si(sample->p[0][j], rand()%100); } /* compute the corresponding value for the full-dimensional constraints */ valuesWithoutElim(sample, elimParms, &smallerSample); /* (N' i' 1)^T = VLinv.(N i 1)^T*/ for (r = 0; r < nbParms; r++) { Inner_Product(&(VLInv->p[r][0]), smallerSample->p[0], nbParms, &(transfSample->p[0][r])); /* add the constant part */ value_addto(transfSample->p[0][r], transfSample->p[0][r], VLInv->p[r][VLInv->NbColumns-2]); value_pdivision(div, transfSample->p[0][r], VLInv->p[r][VLInv->NbColumns-1]); value_subtract(mod, transfSample->p[0][r], div); /* if the parameters value does not belong to the validity lattice, the Ehrhart polynomial is zero. */ if (!value_zero_p(mod)) { fullEP = Enumeration_zero(nbParms, maxRays); break; } } /* compare the two forms of the Ehrhart polynomial.*/ if (origEP ==NULL) break; /* NULL has loose semantics for EPs */ origVal = compute_poly(origEP, sample->p[0]); fullVal = compute_poly(fullEP, transfSample->p[0]); if (!value_eq(*origVal, *fullVal)) { isOk = 0; printf("EPs don't match. \n Original value = "); value_print(stdout, VALUE_FMT, *origVal); printf("\n Original sample = ["); for (j=0; j<sample->NbColumns; j++) { value_print(stdout, VALUE_FMT, sample->p[0][j]); printf(" "); } printf("] \n EP = "); if(origEP!=NULL) { print_evalue(stdout, &(origEP->EP), origNames); } else { printf("NULL"); } printf(" \n Full-dimensional value = "); value_print(stdout, P_VALUE_FMT, *fullVal); printf("\n full-dimensional sample = ["); for (j=0; j<sample->NbColumns; j++) { value_print(stdout, VALUE_FMT, transfSample->p[0][j]); printf(" "); } printf("] \n EP = "); if(origEP!=NULL) { print_evalue(stdout, &(origEP->EP), fullNames); } else { printf("NULL"); } } if (dbg) { printf("\nOriginal value = "); value_print(stdout, VALUE_FMT, *origVal); printf("\nFull-dimensional value = "); value_print(stdout, P_VALUE_FMT, *fullVal); printf("\n"); } value_clear(*origVal); value_clear(*fullVal); } value_clear(mod); value_clear(div); Matrix_Free(sample); Matrix_Free(smallerSample); Matrix_Free(transfSample); Enumeration_Free(origEP); Enumeration_Free(fullEP); return isOk; } /* test_Constraints_fullDimensionize */
/// //Renders a gameobject as it's mesh. // //Parameters: // GO: Game object to render void RenderingManager_Render(LinkedList* gameObjects) { //Clear buffers glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //Set directional light glProgramUniform3fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->directionalLightVectorLocation, 1, renderingBuffer->directionalLightVector->components); Matrix modelMatrix; Matrix_INIT_ON_STACK(modelMatrix, 4, 4); Matrix viewMatrix; Matrix_INIT_ON_STACK(viewMatrix, 4, 4); Matrix modelViewProjectionMatrix; Matrix_INIT_ON_STACK(modelViewProjectionMatrix, 4, 4); //Turn camera's frame of reference into view matrix Camera_ToMatrix4(renderingBuffer->camera, &viewMatrix); //Set viewMatrix uniform glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->viewMatrixLocation, 1, GL_TRUE, viewMatrix.components); //Set projectionMatrix Uniform glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->projectionMatrixLocation, 1, GL_TRUE, renderingBuffer->camera->projectionMatrix->components); struct LinkedList_Node* current = gameObjects->head; while (current != NULL) { GObject* gameObj = (GObject*)(current->data); //Render gameobject's mesh if it exists if (gameObj->mesh != NULL) { //Set color matrix glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->colorMatrixLocation, 1, GL_TRUE, gameObj->colorMatrix->components); //Set modelMatrix uniform FrameOfReference_ToMatrix4(gameObj->frameOfReference, &modelMatrix); glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->modelMatrixLocation, 1, GL_TRUE, modelMatrix.components); //Construct modelViewProjectionMatrix Matrix_Copy(&modelViewProjectionMatrix, &modelMatrix); Matrix_TransformMatrix(&viewMatrix, &modelViewProjectionMatrix); Matrix_TransformMatrix(renderingBuffer->camera->projectionMatrix, &modelViewProjectionMatrix); //Set modelViewProjectionMatrix uniform glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->modelViewProjectionMatrixLocation, 1, GL_TRUE, modelViewProjectionMatrix.components); if (gameObj->texture != NULL) { glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, gameObj->texture->textureID); //Send texture to uniform glUniform1i(renderingBuffer->shaderPrograms[0]->textureLocation, 0); } else { glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, AssetManager_LookupTexture("Test")->textureID); //Send texture to uniform glUniform1i(renderingBuffer->shaderPrograms[0]->textureLocation, 0); } //Setup GPU program to draw this mesh Mesh_Render(gameObj->mesh, gameObj->mesh->primitive); } //Render gameObject's collider if it exists & in debug mode if(gameObj->collider != NULL && gameObj->collider->debug) { //Set color matrix glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->colorMatrixLocation, 1, GL_TRUE, gameObj->collider->colorMatrix->components); //Create modelMatrix from correct Frame Of Reference if(gameObj->body != NULL) { FrameOfReference_ToMatrix4(gameObj->body->frame, &modelMatrix); } else { FrameOfReference_ToMatrix4(gameObj->frameOfReference, &modelMatrix); } //If the object has an AABB collider, take into account the offset if(gameObj->collider->type == COLLIDER_AABB) { ColliderData_AABB* AABB = gameObj->collider->data->AABBData; *Matrix_Index(&modelMatrix, 0, 3) += AABB->centroid->components[0]; *Matrix_Index(&modelMatrix, 1, 3) += AABB->centroid->components[1]; *Matrix_Index(&modelMatrix, 2, 3) += AABB->centroid->components[2]; } //Set the modelMatrix glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->modelMatrixLocation, 1, GL_TRUE, modelMatrix.components); //Construct modelViewProjectionMatrix Matrix_Copy(&modelViewProjectionMatrix, &modelMatrix); Matrix_TransformMatrix(&viewMatrix, &modelViewProjectionMatrix); Matrix_TransformMatrix(renderingBuffer->camera->projectionMatrix, &modelViewProjectionMatrix); //Set modelViewProjectionMatrix uniform glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->modelViewProjectionMatrixLocation, 1, GL_TRUE, modelViewProjectionMatrix.components); //Bind white texture glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, AssetManager_LookupTexture("White")->textureID); //Send texture to uniform glUniform1i(renderingBuffer->shaderPrograms[0]->textureLocation, 0); //Setup GPU program to draw this mesh Mesh_Render(gameObj->collider->representation, GL_LINES); //TODO: Remove //Change the color of colliders to green until they collide *Matrix_Index(gameObj->collider->colorMatrix, 0, 0) = 0.0f; *Matrix_Index(gameObj->collider->colorMatrix, 1, 1) = 1.0f; *Matrix_Index(gameObj->collider->colorMatrix, 2, 2) = 0.0f; } current = current->next; } //Render the oct tree if(renderingBuffer->debugOctTree) { //Set the color matrix Matrix octTreeColor; Matrix_INIT_ON_STACK(octTreeColor, 4, 4); *Matrix_Index(&octTreeColor, 0, 0) = 0.0f; *Matrix_Index(&octTreeColor, 1, 1) = 1.0f; *Matrix_Index(&octTreeColor, 2, 2) = 0.0f; glProgramUniformMatrix4fv(renderingBuffer->shaderPrograms[0]->shaderProgramID, renderingBuffer->shaderPrograms[0]->colorMatrixLocation, 1, GL_TRUE, octTreeColor.components); Mesh* cube = AssetManager_LookupMesh("CubeWire"); Texture* white = AssetManager_LookupTexture("White"); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, white->textureID); //Send texture to uniform glUniform1i(renderingBuffer->shaderPrograms[0]->textureLocation, 0); RenderingManager_RenderOctTree(ObjectManager_GetObjectBuffer().octTree->root, &modelViewProjectionMatrix, &viewMatrix, renderingBuffer->camera->projectionMatrix, cube); } //Start drawing threads on gpu glFlush(); }
ZPolyhedron *SplitZpolyhedron(ZPolyhedron *ZPol,Lattice *B) { Lattice *Intersection = NULL; Lattice *B1 = NULL, *B2 = NULL, *newB1 = NULL, *newB2 = NULL; Matrix *U = NULL,*M1 = NULL, *M2 = NULL, *M1Inverse = NULL,*MtProduct = NULL; Matrix *Vinv, *V , *temp, *DiagMatrix ; Matrix *H , *U1 , *X, *Y ; ZPolyhedron *zpnew, *Result; LatticeUnion *Head = NULL, *tempHead = NULL; int i; Value k; #ifdef DOMDEBUG FILE *fp; fp = fopen("_debug", "a"); fprintf(fp,"\nEntered SplitZpolyhedron \n"); fclose(fp); #endif if (B->NbRows != B->NbColumns) { fprintf(stderr,"\n SplitZpolyhedron : The Input Matrix B is not a proper Lattice \n"); return NULL; } if (ZPol->Lat->NbRows != B->NbRows) { fprintf(stderr,"\nSplitZpolyhedron : The Lattice in Zpolyhedron and B have "); fprintf(stderr,"incompatible dimensions \n"); return NULL; } if (isinHnf (ZPol->Lat) != True) { AffineHermite(ZPol->Lat,&H,&U1); X = Matrix_Copy(H); Matrix_Free(U1); Matrix_Free(H); } else X = Matrix_Copy(ZPol->Lat); if (isinHnf(B) != True) { AffineHermite(B,&H,&U1); Y = Matrix_Copy(H); Matrix_Free(H); Matrix_Free(U1); } else Y = Matrix_Copy(B); if (isEmptyLattice(X)) { return NULL; } Head=Lattice2LatticeUnion(X,Y); /* If the spliting operation can't be done the result is the original Zplyhedron. */ if (Head == NULL) { Matrix_Free(X); Matrix_Free(Y); return ZPol; } Result=NULL; if (Head) while(Head) { tempHead = Head; Head = Head->next; zpnew=ZPolyhedron_Alloc(tempHead->M,ZPol->P); Result=AddZPoly2ZDomain(zpnew,Result); ZPolyhedron_Free(zpnew); tempHead->next = NULL; free(tempHead); } return Result; }
/* * Return the simplified representation of the Z-domain 'ZDom'. It attempts to * convexize unions of polyhedra when they correspond to the same lattices and * to simplify union of lattices when they correspond to the same polyhdera. */ ZPolyhedron *ZDomainSimplify(ZPolyhedron *ZDom) { ZPolyhedron *Ztmp, *Result; ForSimplify *Head, *Prev, *Curr; ZPolyhedron *ZDomHead, *Emp; if (ZDom == NULL) { fprintf(stderr,"\nError in ZDomainSimplify - ZDomHead = NULL\n"); return NULL; } if (ZDom->next == NULL) return (ZPolyhedron_Copy (ZDom)); Emp = EmptyZPolyhedron(ZDom->Lat->NbRows-1); ZDomHead = ZDomainUnion(ZDom, Emp); ZPolyhedron_Free(Emp); Head = NULL; Ztmp = ZDomHead; do { Polyhedron *Img; Img = DomainImage(Ztmp->P,Ztmp->Lat,MAXNOOFRAYS); for(Curr = Head; Curr != NULL; Curr = Curr->next) { Polyhedron *Diff1; Bool flag = False; Diff1 = DomainDifference(Img,Curr->Pol,MAXNOOFRAYS); if (emptyQ(Diff1)) { Polyhedron *Diff2; Diff2 = DomainDifference(Curr->Pol,Img,MAXNOOFRAYS); if (emptyQ(Diff2)) flag = True; Domain_Free(Diff2); } Domain_Free (Diff1); if (flag == True) { LatticeUnion *temp; temp = (LatticeUnion *)malloc(sizeof(LatticeUnion)); temp->M = (Lattice *)Matrix_Copy((Matrix *)Ztmp->Lat); temp->next = Curr->LatUni; Curr->LatUni = temp; break; } } if(Curr == NULL) { Curr = (ForSimplify *)malloc(sizeof(ForSimplify)); Curr->Pol = Domain_Copy(Img); Curr->LatUni = (LatticeUnion *)malloc(sizeof(LatticeUnion)); Curr->LatUni->M = (Lattice *)Matrix_Copy((Matrix *)Ztmp->Lat); Curr->LatUni->next = NULL; Curr->next = Head; Head = Curr; } Domain_Free (Img); Ztmp = Ztmp->next; } while(Ztmp != NULL); for (Curr = Head; Curr != NULL; Curr = Curr->next) Curr->LatUni = LatticeSimplify(Curr->LatUni); Result = NULL; for(Curr = Head; Curr != NULL; Curr = Curr->next) { LatticeUnion *L; for(L = Curr->LatUni; L != NULL; L = L->next) { Polyhedron *Preim; ZPolyhedron *Zpol; Preim = DomainPreimage(Curr->Pol,L->M,MAXNOOFRAYS); Zpol = ZPolyhedron_Alloc(L->M, Preim); Zpol->next = Result; Result = Zpol; Domain_Free(Preim); } } Curr = Head; while (Curr != NULL) { Prev = Curr; Curr = Curr->next; LatticeUnion_Free(Prev->LatUni); Domain_Free(Prev->Pol); free(Prev); } return Result; } /* ZDomainSimplify */
/** extracts the equalities involving the parameters only, try to introduce them back and compare the two polyhedra. Reads a polyhedron and a context. */ int test_Constraints_Remove_parm_eqs(Matrix * A, Matrix * B) { int isOk = 1; Matrix * M, *C, *Cp, * Eqs, *M1, *C1; Polyhedron *Pm, *Pc, *Pcp, *Peqs, *Pint; unsigned int * elimParms; printf("----- test_Constraints_Remove_parm_eqs() -----\n"); M1 = Matrix_Copy(A); C1 = Matrix_Copy(B); M = Matrix_Copy(M1); C = Matrix_Copy(C1); /* compute the combined polyhedron */ Pm = Constraints2Polyhedron(M, maxRays); Pc = Constraints2Polyhedron(C, maxRays); Pcp = align_context(Pc, Pm->Dimension, maxRays); Polyhedron_Free(Pc); Pc = DomainIntersection(Pm, Pcp, maxRays); Polyhedron_Free(Pm); Polyhedron_Free(Pcp); Matrix_Free(M); Matrix_Free(C); /* extract the parm-equalities, expressed in the combined space */ Eqs = Constraints_Remove_parm_eqs(&M1, &C1, 1, &elimParms); printf("Removed equalities: \n"); show_matrix(Eqs); printf("Polyhedron without equalities involving only parameters: \n"); show_matrix(M1); printf("Context without equalities: \n"); show_matrix(C1); /* compute the supposedly-same polyhedron, using the extracted equalities */ Pm = Constraints2Polyhedron(M1, maxRays); Pcp = Constraints2Polyhedron(C1, maxRays); Peqs = align_context(Pcp, Pm->Dimension, maxRays); Polyhedron_Free(Pcp); Pcp = DomainIntersection(Pm, Peqs, maxRays); Polyhedron_Free(Peqs); Polyhedron_Free(Pm); Peqs = Constraints2Polyhedron(Eqs, maxRays); Matrix_Free(Eqs); Matrix_Free(M1); Matrix_Free(C1); Pint = DomainIntersection(Pcp, Peqs, maxRays); Polyhedron_Free(Pcp); Polyhedron_Free(Peqs); /* test their equality */ if (!PolyhedronIncludes(Pint, Pc)) { isOk = 0; } else { if (!PolyhedronIncludes(Pc, Pint)) { isOk = 0; } } Polyhedron_Free(Pc); Polyhedron_Free(Pint); return isOk; } /* test_Constraints_Remove_parm_eqs() */
void render(void) { SVGA3dTextureState *ts; SVGA3dRenderState *rs; SVGA3dVertexDecl *decls; SVGA3dPrimitiveRange *ranges; static Matrix view; Matrix_Copy(view, gIdentityMatrix); Matrix_Scale(view, 0.5, 0.5, 0.5, 1.0); if (lastMouseState.buttons & VMMOUSE_LEFT_BUTTON) { Matrix_RotateX(view, lastMouseState.y * 0.0001); Matrix_RotateY(view, lastMouseState.x * -0.0001); } else { Matrix_RotateX(view, 30.0 * M_PI / 180.0); Matrix_RotateY(view, gFPS.frame * 0.01f); } Matrix_Translate(view, 0, 0, 3); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_VIEW, view); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_WORLD, gIdentityMatrix); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_PROJECTION, perspectiveMat); SVGA3D_BeginSetRenderState(CID, &rs, 4); { rs[0].state = SVGA3D_RS_BLENDENABLE; rs[0].uintValue = FALSE; rs[1].state = SVGA3D_RS_ZENABLE; rs[1].uintValue = TRUE; rs[2].state = SVGA3D_RS_ZWRITEENABLE; rs[2].uintValue = TRUE; rs[3].state = SVGA3D_RS_ZFUNC; rs[3].uintValue = SVGA3D_CMP_LESS; } SVGA_FIFOCommitAll(); SVGA3D_BeginSetTextureState(CID, &ts, 4); { ts[0].stage = 0; ts[0].name = SVGA3D_TS_BIND_TEXTURE; ts[0].value = SVGA3D_INVALID_ID; ts[1].stage = 0; ts[1].name = SVGA3D_TS_COLOROP; ts[1].value = SVGA3D_TC_SELECTARG1; ts[2].stage = 0; ts[2].name = SVGA3D_TS_COLORARG1; ts[2].value = SVGA3D_TA_DIFFUSE; ts[3].stage = 0; ts[3].name = SVGA3D_TS_ALPHAARG1; ts[3].value = SVGA3D_TA_DIFFUSE; } SVGA_FIFOCommitAll(); SVGA3D_BeginDrawPrimitives(CID, &decls, 2, &ranges, 1); { decls[0].identity.type = SVGA3D_DECLTYPE_FLOAT3; decls[0].identity.usage = SVGA3D_DECLUSAGE_POSITION; decls[0].array.surfaceId = vertexSid; decls[0].array.stride = sizeof(MyVertex); decls[0].array.offset = offsetof(MyVertex, position); decls[1].identity.type = SVGA3D_DECLTYPE_D3DCOLOR; decls[1].identity.usage = SVGA3D_DECLUSAGE_COLOR; decls[1].array.surfaceId = vertexSid; decls[1].array.stride = sizeof(MyVertex); decls[1].array.offset = offsetof(MyVertex, color); ranges[0].primType = SVGA3D_PRIMITIVE_TRIANGLELIST; ranges[0].primitiveCount = numTriangles; ranges[0].indexArray.surfaceId = indexSid; ranges[0].indexArray.stride = sizeof(uint16); ranges[0].indexWidth = sizeof(uint16); } SVGA_FIFOCommitAll(); }
/* Return information if the solution was computed */ ptrdiff_t Basis_Solve(PT_Basis pB, double *q, double *x, T_Options options) { /* Basis B */ /* I = W union (Z+m) -> ordered */ /* X = M_/W,Z <- invertible */ /* G = M_W,Z */ /* Bx_B = q -> x = [w_W;z_Z] */ /* z_Z = iX*q_/W */ /* w_W = q_W - G*x_/W */ ptrdiff_t i, incx=1, nb, m, diml, info; char T; double alpha=-1.0, beta=1.0; /* x(1:length(W)) = q(W) */ for(i=0;i<Index_Length(pB->pW);i++) x[i] = q[Index_Get(pB->pW, i)]; /* X = [] */ if(Index_Length(pB->pWc) == 0) return 0; /* z = q(B.Wc) */ for(i=0;i<Index_Length(pB->pWc);i++) pB->z[i] = q[Index_Get(pB->pWc, i)]; /* because the right hand side will be overwritten in dgesv, store it into temporary vector r */ dcopy(&(Index_Length(pB->pWc)), pB->z, &incx, pB->r, &incx); /* printf("x :\n"); */ /* Vector_Print_raw(pB->z,Index_Length(pB->pW)); */ /* printf("z :\n"); */ /* Vector_Print_raw(pB->z,Index_Length(pB->pWc)); */ /* x = [x1;x2] */ /* x2 = inv(X)*q(Wc) = inv(X)*z */ nb = 1; /* number of right hand side in A*x=b is 1 */ m = Matrix_Rows(pB->pF); /* Find solution to general problem A*x=b using LAPACK All the arguments have to be pointers. A and b will be altered on exit. */ switch (options.routine) { case 1: /* DGESV method implements LU factorization of A. */ dgesv(&m, &nb, pMAT(pB->pF), &((pB->pF)->rows_alloc), pB->p, pB->r, &m, &info); break; case 2: /* DGELS solves overdetermined or underdetermined real linear systems involving an M-by-N matrix A, or its transpose, using a QR or LQ factorization of A. It is assumed that A has full rank. */ T = 'n'; /* Not transposed */ diml = 2*m; dgels(&T, &m, &m, &nb, pMAT(pB->pF), &((pB->pF)->rows_alloc), pB->r, &m, pB->s, &diml, &info ); break; default : /* solve with factors F or U */ /* solve using LUMOD (no checks) */ /* LU_Solve0(pB->pLX, pB->pUX, pB->z, &(x[Index_Length(pB->pW)])); */ /* solve using LAPACK (contains also singularity checks) */ /* need to pass info as a pointer, otherwise weird numbers are assigned */ LU_Solve1(pB->pLX, pB->pUt, pB->z, &(x[Index_Length(pB->pW)]), &info); /* if something went wrong, refactorize and solve again */ if (info!=0) { /* printf("info=%ld, refactoring\n", info); */ /* Matrix_Print_row(pB->pLX); */ /* Matrix_Print_utril_row(pB->pUX); */ /* Matrix_Print_col(pB->pUt); */ LU_Refactorize(pB); /* if this fails again, then no minimum ratio is found -> exit with * numerical problems flag */ LU_Solve1(pB->pLX, pB->pUt, pB->z, &(x[Index_Length(pB->pW)]), &info); } } /* x1 = -G*x2 + q(W) */ /* y = alpha*G*x + beta*y */ /* alpha = -1.0; */ /* beta = 1.0; */ T = 'n'; /* Not transposed */ if (options.routine>0) { /* take LAPACK solution */ /* printf("lapack solution z:\n"); */ /* Vector_Print_raw(pB->r,Index_Length(pB->pWc)); */ /* matrix F after solution is factored in [L\U], we want the original format for the next call to dgesv, thus create a copy F <- X */ Matrix_Copy(pB->pX, pB->pF, pB->w); /* printf("x after lapack solution:\n"); */ /* Vector_Print_raw(x,Index_Length(pB->pW)+Index_Length(pB->pWc)); */ /* recompute the remaining variables according to basic solution */ dgemv(&T, &(Matrix_Rows(pB->pG)), &(Matrix_Cols(pB->pG)), &alpha, pMAT(pB->pG), &((pB->pG)->rows_alloc), pB->r, &incx, &beta, x, &incx); /* append the basic solution at the end of x vector */ dcopy(&(Index_Length(pB->pWc)), pB->r, &incx, &(x[Index_Length(pB->pW)]), &incx); } else { /* take LUmod solution */ dgemv(&T, &(Matrix_Rows(pB->pG)), &(Matrix_Cols(pB->pG)), &alpha, pMAT(pB->pG), &((pB->pG)->rows_alloc), &(x[Index_Length(pB->pW)]), &incx, &beta, x, &incx); } /* printf("y:\n"); */ /* Vector_Print_raw(x,Matrix_Rows(pB->pG)); */ return info; }
/* * Return the Z-polyhderon 'Zpol' in canonical form: 'Result' (for the Z-poly- * hedron in canonical form) and Basis 'Basis' (for the basis with respect to * which 'Result' is in canonical form. */ void CanonicalForm(ZPolyhedron *Zpol,ZPolyhedron **Result,Matrix **Basis) { Matrix *B1 = NULL, *B2=NULL, *T1 , *B2inv; int i, l1, l2; Value tmp; Polyhedron *Image, *ImageP; Matrix *H, *U, *temp, *Hprime, *Uprime, *T2; #ifdef DOMDEBUG FILE *fp; fp = fopen("_debug", "a"); fprintf(fp,"\nEntered CANONICALFORM\n"); fclose(fp); #endif if(isEmptyZPolyhedron (Zpol)) { Basis[0] = Identity(Zpol->Lat->NbRows); Result[0] = ZDomain_Copy (Zpol); return ; } value_init(tmp); l1 = FindHermiteBasisofDomain(Zpol->P,&B1); Image = DomainImage (Zpol->P,(Matrix *)Zpol->Lat,MAXNOOFRAYS); l2 = FindHermiteBasisofDomain(Image,&B2); if (l1 != l2) fprintf(stderr,"In CNF : Something wrong with the Input Zpolyhedra \n"); B2inv = Matrix_Alloc(B2->NbRows, B2->NbColumns); temp = Matrix_Copy(B2); Matrix_Inverse(temp,B2inv); Matrix_Free(temp); temp = Matrix_Alloc(B2inv->NbRows,Zpol->Lat->NbColumns); T1 = Matrix_Alloc(temp->NbRows,B1->NbColumns); Matrix_Product(B2inv,(Matrix *)Zpol->Lat,temp); Matrix_Product(temp,B1,T1); Matrix_Free(temp); T2 = ChangeLatticeDimension(T1,l1); temp = ChangeLatticeDimension(T2,T2->NbRows+1); /* Adding the affine part */ for(i = 0; i < l1; i ++) value_assign(temp->p[i][temp->NbColumns-1],T1->p[i][T1->NbColumns-1]); AffineHermite(temp,&H,&U); Hprime = ChangeLatticeDimension(H,Zpol->Lat->NbRows); /* Exchanging the Affine part */ for(i = 0; i < l1; i ++) { value_assign(tmp,Hprime->p[i][Hprime->NbColumns-1]); value_assign(Hprime->p[i][Hprime->NbColumns-1],Hprime->p[i][H->NbColumns-1]); value_assign(Hprime->p[i][H->NbColumns-1],tmp); } Uprime = ChangeLatticeDimension(U,Zpol->Lat->NbRows); /* Exchanging the Affine part */ for (i = 0;i < l1; i++) { value_assign(tmp,Uprime->p[i][Uprime->NbColumns-1]); value_assign(Uprime->p[i][Uprime->NbColumns-1],Uprime->p[i][U->NbColumns-1]); value_assign(Uprime->p[i][U->NbColumns-1],tmp); } Polyhedron_Free (Image); Matrix_Free (B2inv); B2inv = Matrix_Alloc(B1->NbRows, B1->NbColumns); Matrix_Inverse(B1,B2inv); ImageP = DomainImage(Zpol->P, B2inv, MAXNOOFRAYS); Matrix_Free(B2inv); Image = DomainImage(ImageP, Uprime, MAXNOOFRAYS); Domain_Free(ImageP); Result[0] = ZPolyhedron_Alloc(Hprime, Image); Basis[0] = Matrix_Copy(B2); /* Free the variables */ Polyhedron_Free (Image); Matrix_Free (B1); Matrix_Free (B2); Matrix_Free (temp); Matrix_Free (T1); Matrix_Free (T2); Matrix_Free (H); Matrix_Free (U); Matrix_Free (Hprime); Matrix_Free (Uprime); value_clear(tmp); return; } /* CanonicalForm */
static void drawCube(void) { static float angle = 0.5f; SVGA3dRect *rect; Matrix perspectiveMat; SVGA3dTextureState *ts; SVGA3dRenderState *rs; SVGA3dRect viewport = { 0, 0, surfWidth, surfHeight }; SVGA3D_SetRenderTarget(CID, SVGA3D_RT_COLOR0, &colorImage); SVGA3D_SetRenderTarget(CID, SVGA3D_RT_DEPTH, &depthImage); SVGA3D_SetViewport(CID, &viewport); SVGA3D_SetZRange(CID, 0.0f, 1.0f); SVGA3D_BeginSetRenderState(CID, &rs, 5); { rs[0].state = SVGA3D_RS_BLENDENABLE; rs[0].uintValue = FALSE; rs[1].state = SVGA3D_RS_ZENABLE; rs[1].uintValue = TRUE; rs[2].state = SVGA3D_RS_ZWRITEENABLE; rs[2].uintValue = TRUE; rs[3].state = SVGA3D_RS_ZFUNC; rs[3].uintValue = SVGA3D_CMP_LESS; rs[4].state = SVGA3D_RS_LIGHTINGENABLE; rs[4].uintValue = FALSE; } SVGA_FIFOCommitAll(); SVGA3D_BeginSetTextureState(CID, &ts, 4); { ts[0].stage = 0; ts[0].name = SVGA3D_TS_BIND_TEXTURE; ts[0].value = SVGA3D_INVALID_ID; ts[1].stage = 0; ts[1].name = SVGA3D_TS_COLOROP; ts[1].value = SVGA3D_TC_SELECTARG1; ts[2].stage = 0; ts[2].name = SVGA3D_TS_COLORARG1; ts[2].value = SVGA3D_TA_DIFFUSE; ts[3].stage = 0; ts[3].name = SVGA3D_TS_ALPHAARG1; ts[3].value = SVGA3D_TA_DIFFUSE; } SVGA_FIFOCommitAll(); /* * Draw a red border around the render target, to test edge * accuracy in Present. */ SVGA3D_BeginClear(CID, SVGA3D_CLEAR_COLOR | SVGA3D_CLEAR_DEPTH, 0xFF0000, 1.0f, 0, &rect, 1); *rect = viewport; SVGA_FIFOCommitAll(); /* * Draw the background color */ SVGA3D_BeginClear(CID, SVGA3D_CLEAR_COLOR | SVGA3D_CLEAR_DEPTH, 0x336699, 1.0f, 0, &rect, 1); rect->x = viewport.x + 1; rect->y = viewport.y + 1; rect->w = viewport.w - 2; rect->h = viewport.h - 2; SVGA_FIFOCommitAll(); SVGA3dVertexDecl *decls; SVGA3dPrimitiveRange *ranges; Matrix view; Matrix_Copy(view, gIdentityMatrix); Matrix_Scale(view, 0.5, 0.5, 0.5, 1.0); Matrix_RotateX(view, 30.0 * M_PI / 180.0); Matrix_RotateY(view, angle); Matrix_Translate(view, 0, 0, 2.2); angle += 0.02; Matrix_Perspective(perspectiveMat, 45.0f, 4.0f / 3.0f, 0.1f, 100.0f); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_WORLD, gIdentityMatrix); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_PROJECTION, perspectiveMat); SVGA3D_SetTransform(CID, SVGA3D_TRANSFORM_VIEW, view); SVGA3D_BeginDrawPrimitives(CID, &decls, 2, &ranges, 1); { decls[0].identity.type = SVGA3D_DECLTYPE_FLOAT3; decls[0].identity.usage = SVGA3D_DECLUSAGE_POSITION; decls[0].array.surfaceId = vertexSid; decls[0].array.stride = sizeof(MyVertex); decls[0].array.offset = offsetof(MyVertex, position); decls[1].identity.type = SVGA3D_DECLTYPE_D3DCOLOR; decls[1].identity.usage = SVGA3D_DECLUSAGE_COLOR; decls[1].array.surfaceId = vertexSid; decls[1].array.stride = sizeof(MyVertex); decls[1].array.offset = offsetof(MyVertex, color); ranges[0].primType = SVGA3D_PRIMITIVE_LINELIST; ranges[0].primitiveCount = numLines; ranges[0].indexArray.surfaceId = indexSid; ranges[0].indexArray.stride = sizeof(uint16); ranges[0].indexWidth = sizeof(uint16); } SVGA_FIFOCommitAll(); }