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