int
main(int argc, char **argv)
{
/* Command-line parsing variables. */
int arg; /* command-line argument counter */
static LALStatus stat; /* status structure */
CHAR *sourcefile = NULL; /* name of sourcefile */
CHAR *respfile = NULL; /* name of respfile */
CHAR *infile = NULL; /* name of infile */
CHAR *outfile = NULL; /* name of outfile */
INT4 seed = 0; /* random number seed */
INT4 sec = SEC; /* ouput epoch.gpsSeconds */
INT4 nsec = NSEC; /* ouput epoch.gpsNanoSeconds */
INT4 npt = NPT; /* number of output points */
REAL8 dt = DT; /* output sampling interval */
REAL4 sigma = SIGMA; /* noise amplitude */
/* File reading variables. */
FILE *fp = NULL; /* generic file pointer */
BOOLEAN ok = 1; /* whether input format is correct */
UINT4 i; /* generic index over file lines */
INT8 epoch; /* epoch stored as an INT8 */
/* Other global variables. */
RandomParams *params = NULL; /* parameters of pseudorandom sequence */
DetectorResponse detector; /* the detector in question */
INT2TimeSeries output; /* detector ACD output */
/*******************************************************************
* PARSE ARGUMENTS (arg stores the current position) *
*******************************************************************/
/* Exit gracefully if no arguments were given.
if ( argc <= 1 ) {
INFO( "No testing done." );
return 0;
} */
arg = 1;
while ( arg < argc ) {
/* Parse source file option. */
if ( !strcmp( argv[arg], "-s" ) ) {
if ( argc > arg + 1 ) {
arg++;
sourcefile = argv[arg++];
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Parse response file option. */
else if ( !strcmp( argv[arg], "-r" ) ) {
if ( argc > arg + 1 ) {
arg++;
respfile = argv[arg++];
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Parse input file option. */
else if ( !strcmp( argv[arg], "-i" ) ) {
if ( argc > arg + 1 ) {
arg++;
infile = argv[arg++];
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Parse noise output option. */
else if ( !strcmp( argv[arg], "-n" ) ) {
if ( argc > arg + 5 ) {
arg++;
sec = atoi( argv[arg++] );
nsec = atoi( argv[arg++] );
npt = atoi( argv[arg++] );
dt = atof( argv[arg++] );
sigma = atof( argv[arg++] );
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Parse output file option. */
else if ( !strcmp( argv[arg], "-o" ) ) {
if ( argc > arg + 1 ) {
arg++;
outfile = argv[arg++];
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Parse debug level option. */
else if ( !strcmp( argv[arg], "-d" ) ) {
if ( argc > arg + 1 ) {
arg++;
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Parse random seed option. */
else if ( !strcmp( argv[arg], "-e" ) ) {
if ( argc > arg + 1 ) {
arg++;
seed = atoi( argv[arg++] );
}else{
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
}
/* Check for unrecognized options. */
else if ( argv[arg][0] == '-' ) {
ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return BASICINJECTTESTC_EARG;
}
} /* End of argument parsing loop. */
/* Check for redundant or bad argument values. */
CHECKVAL( npt, 0, 2147483647 );
CHECKVAL( dt, 0, LAL_REAL4_MAX );
/*******************************************************************
* SETUP *
*******************************************************************/
/* Set up output, detector, and random parameter structures. */
output.data = NULL;
detector.transfer = (COMPLEX8FrequencySeries *)
LALMalloc( sizeof(COMPLEX8FrequencySeries) );
if ( !(detector.transfer) ) {
ERROR( BASICINJECTTESTC_EMEM, BASICINJECTTESTC_MSGEMEM, 0 );
return BASICINJECTTESTC_EMEM;
}
detector.transfer->data = NULL;
detector.site = NULL;
SUB( LALCreateRandomParams( &stat, ¶ms, seed ), &stat );
/* Set up units. */
output.sampleUnits = lalADCCountUnit;
if (XLALUnitDivide( &(detector.transfer->sampleUnits),
&lalADCCountUnit, &lalStrainUnit ) == NULL) {
return LAL_EXLAL;
}
/* Read response function. */
if ( respfile ) {
REAL4VectorSequence *resp = NULL; /* response as vector sequence */
COMPLEX8Vector *response = NULL; /* response as complex vector */
COMPLEX8Vector *unity = NULL; /* vector of complex 1's */
if ( ( fp = fopen( respfile, "r" ) ) == NULL ) {
ERROR( BASICINJECTTESTC_EFILE, BASICINJECTTESTC_MSGEFILE,
respfile );
return BASICINJECTTESTC_EFILE;
}
/* Read header. */
ok &= ( fscanf( fp, "# epoch = %" LAL_INT8_FORMAT "\n", &epoch ) == 1 );
I8ToLIGOTimeGPS( &( detector.transfer->epoch ), epoch );
ok &= ( fscanf( fp, "# f0 = %lf\n", &( detector.transfer->f0 ) )
== 1 );
ok &= ( fscanf( fp, "# deltaF = %lf\n",
&( detector.transfer->deltaF ) ) == 1 );
if ( !ok ) {
ERROR( BASICINJECTTESTC_EINPUT, BASICINJECTTESTC_MSGEINPUT,
respfile );
return BASICINJECTTESTC_EINPUT;
}
/* Read and convert body to a COMPLEX8Vector. */
SUB( LALSReadVectorSequence( &stat, &resp, fp ), &stat );
fclose( fp );
if ( resp->vectorLength != 2 ) {
ERROR( BASICINJECTTESTC_EINPUT, BASICINJECTTESTC_MSGEINPUT,
respfile );
return BASICINJECTTESTC_EINPUT;
}
SUB( LALCCreateVector( &stat, &response, resp->length ), &stat );
memcpy( response->data, resp->data, 2*resp->length*sizeof(REAL4) );
SUB( LALSDestroyVectorSequence( &stat, &resp ), &stat );
/* Convert response function to a transfer function. */
SUB( LALCCreateVector( &stat, &unity, response->length ), &stat );
for ( i = 0; i < response->length; i++ ) {
unity->data[i] = 1.0;
}
SUB( LALCCreateVector( &stat, &( detector.transfer->data ),
response->length ), &stat );
SUB( LALCCVectorDivide( &stat, detector.transfer->data, unity,
response ), &stat );
SUB( LALCDestroyVector( &stat, &response ), &stat );
SUB( LALCDestroyVector( &stat, &unity ), &stat );
}
/* No response file, so generate a unit response. */
else {
I8ToLIGOTimeGPS( &( detector.transfer->epoch ), EPOCH );
detector.transfer->f0 = 0.0;
detector.transfer->deltaF = 1.5*FSTOP;
SUB( LALCCreateVector( &stat, &( detector.transfer->data ), 2 ),
&stat );
detector.transfer->data->data[0] = 1.0;
detector.transfer->data->data[1] = 1.0;
}
/* Read input data. */
if ( infile ) {
REAL4VectorSequence *input = NULL; /* input as vector sequence */
if ( ( fp = fopen( infile, "r" ) ) == NULL ) {
ERROR( BASICINJECTTESTC_EFILE, BASICINJECTTESTC_MSGEFILE,
infile );
return BASICINJECTTESTC_EFILE;
}
/* Read header. */
ok &= ( fscanf( fp, "# epoch = %" LAL_INT8_FORMAT "\n", &epoch ) == 1 );
I8ToLIGOTimeGPS( &( output.epoch ), epoch );
ok &= ( fscanf( fp, "# deltaT = %lf\n", &( output.deltaT ) )
== 1 );
if ( !ok ) {
ERROR( BASICINJECTTESTC_EINPUT, BASICINJECTTESTC_MSGEINPUT,
infile );
return BASICINJECTTESTC_EINPUT;
}
/* Read and convert body. */
SUB( LALSReadVectorSequence( &stat, &input, fp ), &stat );
fclose( fp );
if ( input->vectorLength != 1 ) {
ERROR( BASICINJECTTESTC_EINPUT, BASICINJECTTESTC_MSGEINPUT,
infile );
return BASICINJECTTESTC_EINPUT;
}
SUB( LALI2CreateVector( &stat, &( output.data ), input->length ),
&stat );
for ( i = 0; i < input->length; i++ )
output.data->data[i] = (INT2)( input->data[i] );
SUB( LALSDestroyVectorSequence( &stat, &input ), &stat );
}
/* No input file, so generate one randomly. */
else {
output.epoch.gpsSeconds = sec;
output.epoch.gpsNanoSeconds = nsec;
output.deltaT = dt;
SUB( LALI2CreateVector( &stat, &( output.data ), npt ), &stat );
if ( sigma == 0 ) {
memset( output.data->data, 0, npt*sizeof(INT2) );
} else {
REAL4Vector *deviates = NULL; /* unit Gaussian deviates */
SUB( LALSCreateVector( &stat, &deviates, npt ), &stat );
SUB( LALNormalDeviates( &stat, deviates, params ), &stat );
for ( i = 0; i < (UINT4)( npt ); i++ )
output.data->data[i] = (INT2)
floor( sigma*deviates->data[i] + 0.5 );
SUB( LALSDestroyVector( &stat, &deviates ), &stat );
}
}
/*******************************************************************
* INJECTION *
*******************************************************************/
/* Open sourcefile. */
if ( sourcefile ) {
if ( ( fp = fopen( sourcefile, "r" ) ) == NULL ) {
ERROR( BASICINJECTTESTC_EFILE, BASICINJECTTESTC_MSGEFILE,
sourcefile );
return BASICINJECTTESTC_EFILE;
}
}
/* For each line in the sourcefile... */
while ( ok ) {
PPNParamStruc ppnParams; /* wave generation parameters */
REAL4 m1, m2, dist, inc, phic; /* unconverted parameters */
CoherentGW waveform; /* amplitude and phase structure */
REAL4TimeSeries signalvec; /* GW signal */
REAL8 time; /* length of GW signal */
CHAR timeCode; /* code for signal time alignment */
CHAR message[MSGLEN]; /* warning/info messages */
/* Read and convert input line. */
if ( sourcefile ) {
ok &= ( fscanf( fp, "%c %" LAL_INT8_FORMAT " %f %f %f %f %f\n", &timeCode,
&epoch, &m1, &m2, &dist, &inc, &phic ) == 7 );
ppnParams.mTot = m1 + m2;
ppnParams.eta = m1*m2/( ppnParams.mTot*ppnParams.mTot );
ppnParams.d = dist*LAL_PC_SI*1000.0;
ppnParams.inc = inc*LAL_PI/180.0;
ppnParams.phi = phic*LAL_PI/180.0;
} else {
timeCode = 'i';
ppnParams.mTot = M1 + M2;
ppnParams.eta = M1*M2/( ppnParams.mTot*ppnParams.mTot );
ppnParams.d = DIST;
ppnParams.inc = INC;
ppnParams.phi = PHIC;
epoch = EPOCH;
}
if ( ok ) {
/* Set up other parameter structures. */
ppnParams.epoch.gpsSeconds = ppnParams.epoch.gpsNanoSeconds = 0;
ppnParams.position.latitude = ppnParams.position.longitude = 0.0;
ppnParams.position.system = COORDINATESYSTEM_EQUATORIAL;
ppnParams.psi = 0.0;
ppnParams.fStartIn = FSTART;
ppnParams.fStopIn = FSTOP;
ppnParams.lengthIn = 0;
ppnParams.ppn = NULL;
ppnParams.deltaT = DELTAT;
memset( &waveform, 0, sizeof(CoherentGW) );
/* Generate waveform at zero epoch. */
SUB( LALGeneratePPNInspiral( &stat, &waveform, &ppnParams ),
&stat );
snprintf( message, MSGLEN, "%d: %s", ppnParams.termCode,
ppnParams.termDescription );
INFO( message );
if ( ppnParams.dfdt > 2.0 ) {
snprintf( message, MSGLEN,
"Waveform sampling interval is too large:\n"
"\tmaximum df*dt = %f", ppnParams.dfdt );
WARNING( message );
}
/* Compute epoch for waveform. */
time = waveform.a->data->length*DELTAT;
if ( timeCode == 'f' )
epoch -= (INT8)( 1000000000.0*time );
else if ( timeCode == 'c' )
epoch -= (INT8)( 1000000000.0*ppnParams.tc );
I8ToLIGOTimeGPS( &( waveform.a->epoch ), epoch );
waveform.f->epoch = waveform.phi->epoch = waveform.a->epoch;
/* Generate and inject signal. */
signalvec.epoch = waveform.a->epoch;
signalvec.epoch.gpsSeconds -= 1;
signalvec.deltaT = output.deltaT/4.0;
signalvec.f0 = 0;
signalvec.data = NULL;
time = ( time + 2.0 )/signalvec.deltaT;
SUB( LALSCreateVector( &stat, &( signalvec.data ), (UINT4)time ),
&stat );
SUB( LALSimulateCoherentGW( &stat, &signalvec, &waveform,
&detector ), &stat );
SUB( LALSI2InjectTimeSeries( &stat, &output, &signalvec, params ),
&stat );
SUB( LALSDestroyVectorSequence( &stat, &( waveform.a->data ) ),
&stat );
SUB( LALSDestroyVector( &stat, &( waveform.f->data ) ), &stat );
SUB( LALDDestroyVector( &stat, &( waveform.phi->data ) ), &stat );
LALFree( waveform.a );
LALFree( waveform.f );
LALFree( waveform.phi );
SUB( LALSDestroyVector( &stat, &( signalvec.data ) ), &stat );
}
/* If there is no source file, inject only one source. */
if ( !sourcefile )
ok = 0;
}
/* Input file is exhausted (or has a badly-formatted line ). */
if ( sourcefile )
fclose( fp );
/*******************************************************************
* CLEANUP *
*******************************************************************/
/* Print output file. */
if ( outfile ) {
if ( ( fp = fopen( outfile, "w" ) ) == NULL ) {
ERROR( BASICINJECTTESTC_EFILE, BASICINJECTTESTC_MSGEFILE,
outfile );
return BASICINJECTTESTC_EFILE;
}
epoch = 1000000000LL*(INT8)( output.epoch.gpsSeconds );
epoch += (INT8)( output.epoch.gpsNanoSeconds );
fprintf( fp, "# epoch = %" LAL_INT8_FORMAT "\n", epoch );
fprintf( fp, "# deltaT = %23.16e\n", output.deltaT );
for ( i = 0; i < output.data->length; i++ )
fprintf( fp, "%8.1f\n", (REAL4)( output.data->data[i] ) );
fclose( fp );
}
/* Destroy remaining memory. */
SUB( LALDestroyRandomParams( &stat, ¶ms ), &stat );
SUB( LALI2DestroyVector( &stat, &( output.data ) ), &stat );
SUB( LALCDestroyVector( &stat, &( detector.transfer->data ) ),
&stat );
LALFree( detector.transfer );
/* Done! */
LALCheckMemoryLeaks();
INFO( BASICINJECTTESTC_MSGENORM );
return BASICINJECTTESTC_ENORM;
}
bool triangle_intersection( const Vector3& V1, // Triangle vertices
const Vector3& V2,
const Vector3& V3,
const Vector3& O,
const Vector3& D,
float& out,
float BG[2])
{
Vector3 e1, e2; //Edge1, Edge2
Vector3 P, Q, T;
float det, inv_det, u, v;
float t;
//Find vectors for two edges sharing V1
SUB(e1, V2, V1);
SUB(e2, V3, V1);
//Begin calculating determinant - also used to calculate u parameter
CROSS(P, D, e2);
//if determinant is near zero, ray lies in plane of triangle
det = DOT(e1, P);
//NOT CULLING
if(det > -EPSILON && det < EPSILON) return false;
inv_det = 1.f / det;
//calculate distance from V1 to ray origin
SUB(T, O, V1);
//Calculate u parameter and test bound
u = DOT(T, P) * inv_det;
//The intersection lies outside of the triangle
if(u < 0.f || u > 1.f) return false;
//Prepare to test v parameter
CROSS(Q, T, e1);
//Calculate V parameter and test bound
v = DOT(D, Q) * inv_det;
//The intersection lies outside of the triangle
if(u < 0.f || v < 0.f || u + v > 1.f) return false;
//u and v are the barycentric coordinates
t = DOT(e2, Q) * inv_det;
if(t > EPSILON)
{ //ray intersection
out = t;
BG[0] = u;
BG[1] = v;
return true;
}
// No hit, no win
return false;
}
void updateH(N3 N, P3F3 E, P3F3 H, P3F3 CH) {
int i,j,k;
v4sf h, ch, e1, e2, e3, e4;
//omp_set_num_threads(8);
for (i=1;i<N.x;i++){
for (j=1;j<N.y;j++){
for (k=0;k<N.z;k+=4){
h = LOAD(&H.x[i][j][k]);
ch = LOAD(&CH.x[i][j][k]);
e1 = LOAD(&E.z[i][j][k]);
e2 = LOAD(&E.z[i][j-1][k]);
e3 = LOAD(&E.y[i][j][k]);
e4 = LOAD(&E.y[i][j][k-1]);
STORE(&H.x[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4)))));
}
}
}
for (i=1;i<N.x;i++){
for (j=1;j<N.y;j++){
for (k=0;k<N.z;k+=4){
h = LOAD(&H.y[i][j][k]);
ch = LOAD(&CH.y[i][j][k]);
e1 = LOAD(&E.x[i][j][k]);
e2 = LOAD(&E.x[i][j][k-1]);
e3 = LOAD(&E.z[i][j][k]);
e4 = LOAD(&E.z[i-1][j][k]);
STORE(&H.y[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4)))));
}
}
}
for (i=1;i<N.x;i++){
for (j=1;j<N.y;j++){
for (k=0;k<N.z;k+=4){
h = LOAD(&H.z[i][j][k]);
ch = LOAD(&CH.z[i][j][k]);
e1 = LOAD(&E.y[i][j][k]);
e2 = LOAD(&E.y[i-1][j][k]);
e3 = LOAD(&E.x[i][j][k]);
e4 = LOAD(&E.x[i][j-1][k]);
STORE(&H.z[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4)))));
}
}
}
i=0;
for (j=1;j<N.y;j++){
for (k=0;k<N.z;k+=4){
h = LOAD(&H.x[i][j][k]);
ch = LOAD(&CH.x[i][j][k]);
e1 = LOAD(&E.z[i][j][k]);
e2 = LOAD(&E.z[i][j-1][k]);
e3 = LOAD(&E.y[i][j][k]);
e4 = LOAD(&E.y[i][j][k-1]);
STORE(&H.x[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4)))));
}
}
j=0;
for (i=1;i<N.x;i++){
for (k=0;k<N.z;k+=4){
h = LOAD(&H.y[i][j][k]);
ch = LOAD(&CH.y[i][j][k]);
e1 = LOAD(&E.x[i][j][k]);
e2 = LOAD(&E.x[i][j][k-1]);
e3 = LOAD(&E.z[i][j][k]);
e4 = LOAD(&E.z[i-1][j][k]);
STORE(&H.y[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4)))));
}
}
}
bool8 AtiTriBoxMoller( const TBM_FLOAT boxcenter[3], const TBM_FLOAT boxhalfsize[3],
const TBM_FLOAT triVert0[3], const TBM_FLOAT triVert1[3],
const TBM_FLOAT triVert2[3])
{
// use separating axis theorem to test overlap between triangle and box
// need to test for overlap in these directions:
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
// we do not even need to test these)
// 2) normal of the triangle
// 3) crossproduct(edge from tri, {x,y,z}-directin)
// this gives 3x3=9 more tests
TBM_FLOAT v0[3], v1[3], v2[3];
TBM_FLOAT min, max, d, p0, p1, p2, rad, fex, fey, fez;
TBM_FLOAT normal[3], e0[3], e1[3], e2[3];
// This is the fastest branch on Sun
// move everything so that the boxcenter is in (0,0,0)
SUB (v0, triVert0, boxcenter);
SUB (v1, triVert1, boxcenter);
SUB (v2, triVert2, boxcenter);
// compute triangle edges
SUB (e0, v1, v0); // tri edge 0
SUB (e1, v2, v1); // tri edge 1
SUB (e2, v0, v2); // tri edge 2
// Bullet 3:
// test the 9 tests first (this was faster)
fex = (TBM_FLOAT)fabs (e0[X]);
fey = (TBM_FLOAT)fabs (e0[Y]);
fez = (TBM_FLOAT)fabs (e0[Z]);
AXISTEST_X01 (e0[Z], e0[Y], fez, fey);
AXISTEST_Y02 (e0[Z], e0[X], fez, fex);
AXISTEST_Z12 (e0[Y], e0[X], fey, fex);
fex = (TBM_FLOAT)fabs (e1[X]);
fey = (TBM_FLOAT)fabs (e1[Y]);
fez = (TBM_FLOAT)fabs (e1[Z]);
AXISTEST_X01 (e1[Z], e1[Y], fez, fey);
AXISTEST_Y02 (e1[Z], e1[X], fez, fex);
AXISTEST_Z0 (e1[Y], e1[X], fey, fex);
fex = (TBM_FLOAT)fabs (e2[X]);
fey = (TBM_FLOAT)fabs (e2[Y]);
fez = (TBM_FLOAT)fabs (e2[Z]);
AXISTEST_X2 (e2[Z], e2[Y], fez, fey);
AXISTEST_Y1 (e2[Z], e2[X], fez, fex);
AXISTEST_Z12 (e2[Y], e2[X], fey, fex);
// Bullet 1:
// first test overlap in the {x,y,z}-directions
// find min, max of the triangle each direction, and test for overlap in
// that direction -- this is equivalent to testing a minimal AABB around
// the triangle against the AABB
// test in X-direction
FINDMINMAX (v0[X], v1[X], v2[X], min, max);
if (min > boxhalfsize[X] || max < -boxhalfsize[X])
{
return FALSE;
}
// test in Y-direction
FINDMINMAX (v0[Y], v1[Y], v2[Y], min, max);
if (min > boxhalfsize[Y] || max < -boxhalfsize[Y])
{
return FALSE;
}
// test in Z-direction
FINDMINMAX (v0[Z], v1[Z], v2[Z], min, max);
if (min > boxhalfsize[Z] || max < -boxhalfsize[Z])
{
return FALSE;
}
// Bullet 2:
// test if the box intersects the plane of the triangle
// compute plane equation of triangle: normal*x+d=0
CROSS (normal, e0, e1);
d = -DOT (normal, v0); // plane eq: normal.x+d=0
return AtiPlaneBoxOverlap (normal, d, boxhalfsize);
}
/* vvvvvvvvvvvvvvvvvvvvvvvvvvvvvv------------------------------------ */
int main(int argc, char *argv[]){
static LALStatus status; /* LALStatus pointer */
static LALDetector detector;
static LIGOTimeGPSVector timeV;
static REAL8Cart3CoorVector velV;
static HOUGHptfLUTVector lutV; /* the Look Up Table vector*/
static HOUGHPeakGramVector pgV;
static PHMDVectorSequence phmdVS; /* the partial Hough map derivatives */
static UINT8FrequencyIndexVector freqInd;
static HOUGHResolutionPar parRes;
static HOUGHPatchGrid patch; /* Patch description */
static HOUGHParamPLUT parLut; /* parameters needed to build lut */
static HOUGHDemodPar parDem; /* demodulation parameters or */
static HOUGHSizePar parSize;
static VelocityPar velPar;
static HOUGHMapTotal ht; /* the total Hough map */
/* ------------------------------------------------------- */
CHAR *earthEphemeris = NULL;
CHAR *sunEphemeris = NULL;
INT4 ifo;
REAL8 vel[3];
INT4 mObsCoh;
UINT2 maxNBins, maxNBorders;
INT8 f0Bin; /* freq. bin to construct LUT */
INT8 fBin;
UINT2 xSide, ySide;
CHAR *fname = NULL; /* The output filename */
FILE *fp=NULL; /* Output file */
INT4 arg; /* Argument counter */
UINT4 i,j; /* Index counter, etc */
INT4 k;
REAL8 f0, alpha, delta;
REAL8 patchSizeX, patchSizeY;
REAL8 Xx,Xy,Xz;
/******************************************************************/
/* Set up the default parameters. */
/* ****************************************************************/
detector = lalCachedDetectors[LALDetectorIndexGEO600DIFF]; /* default */
ifo = IFO;
if (ifo ==1) detector=lalCachedDetectors[LALDetectorIndexGEO600DIFF];
if (ifo ==2) detector=lalCachedDetectors[LALDetectorIndexLLODIFF];
if (ifo ==3) detector=lalCachedDetectors[LALDetectorIndexLHODIFF];
earthEphemeris = EARTHEPHEMERIS;
sunEphemeris = SUNEPHEMERIS;
mObsCoh = MOBSCOH;
timeV.length = mObsCoh;
velV.length = mObsCoh;
lutV.length = mObsCoh;
pgV.length = mObsCoh;
phmdVS.length = mObsCoh;
freqInd.length = mObsCoh;
phmdVS.nfSize = NFSIZE;
freqInd.deltaF = DF;
phmdVS.deltaF = DF;
timeV.time = NULL;
velV.data = NULL;
lutV.lut = NULL;
pgV.pg = NULL;
phmdVS.phmd = NULL;
freqInd.data = NULL;
ht.map = NULL;
f0 = F0;
f0Bin = F0*TCOH;
parRes.f0Bin = f0Bin;
parRes.deltaF = DF;
/*
* parRes.patchSkySizeX = patchSizeX = 1.0/(TCOH*F0*VEPI);
* parRes.patchSkySizeY = patchSizeY = 1.0/(TCOH*F0*VEPI);
*/
parRes.patchSkySizeX = patchSizeX = PATCHSIZEX;
parRes.patchSkySizeY = patchSizeY = PATCHSIZEY;
parRes.pixelFactor = PIXELFACTOR;
parRes.pixErr = PIXERR;
parRes.linErr = LINERR;
parRes.vTotC = VTOT;
/* Case: no spins & Non demodulation */
parDem.deltaF = DF;
parDem.skyPatch.alpha = ALPHA;
parDem.skyPatch.delta = DELTA;
parDem.timeDiff = 0.0;
parDem.spin.length = 0;
parDem.spin.data = NULL;
parDem.positC.x = 0.0;
parDem.positC.y = 0.0;
parDem.positC.z = 0.0;
velPar.detector = detector;
velPar.tBase = TCOH;
velPar.vTol = ACCURACY;
alpha = ALPHA;
delta = DELTA;
/*****************************************************************/
/*****************************************************************/
/* Parse argument list. i stores the current position. */
/*****************************************************************/
arg = 1;
while ( arg < argc ) {
/* Parse debuglevel option. */
if ( !strcmp( argv[arg], "-d" ) ) {
if ( argc > arg + 1 ) {
arg++;
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse interferometer option. */
else if ( !strcmp( argv[arg], "-i" ) ) {
if ( argc > arg + 1 ) {
arg++;
ifo = atoi( argv[arg++] );
if (ifo ==1) detector=lalCachedDetectors[LALDetectorIndexGEO600DIFF];
if (ifo ==2) detector=lalCachedDetectors[LALDetectorIndexLLODIFF];
if (ifo ==3) detector=lalCachedDetectors[LALDetectorIndexLHODIFF];
velPar.detector = detector;
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse filename of earth ephemeris data option. */
else if ( !strcmp( argv[arg], "-E" ) ) {
if ( argc > arg + 1 ) {
arg++;
earthEphemeris = argv[arg++];
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse filename of sun ephemeris data option. */
else if ( !strcmp( argv[arg], "-S" ) ) {
if ( argc > arg + 1 ) {
arg++;
sunEphemeris = argv[arg++];
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse output file option. */
else if ( !strcmp( argv[arg], "-o" ) ) {
if ( argc > arg + 1 ) {
arg++;
fname = argv[arg++];
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse frequency option. */
else if ( !strcmp( argv[arg], "-f" ) ) {
if ( argc > arg + 1 ) {
arg++;
f0 = atof(argv[arg++]);
f0Bin = f0*TCOH;
parRes.f0Bin = f0Bin;
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse sky position options. */
else if ( !strcmp( argv[arg], "-p" ) ) {
if ( argc > arg + 2 ) {
arg++;
alpha = atof(argv[arg++]);
delta = atof(argv[arg++]);
parDem.skyPatch.alpha = alpha;
parDem.skyPatch.delta = delta;
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Parse patch size option. */
else if ( !strcmp( argv[arg], "-s" ) ) {
if ( argc > arg + 2 ) {
arg++;
parRes.patchSkySizeX = patchSizeX = atof(argv[arg++]);
parRes.patchSkySizeY = patchSizeY = atof(argv[arg++]);
} else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
}
/* Unrecognized option. */
else {
ERROR( VALIDATION1_EARG, VALIDATION1_MSGEARG, 0 );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EARG;
}
} /* End of argument parsing loop. */
/******************************************************************/
if ( f0 < 0 ) {
ERROR( VALIDATION1_EBAD, VALIDATION1_MSGEBAD, "freq<0:" );
XLALPrintError( USAGE, *argv );
return VALIDATION1_EBAD;
}
/******************************************************************/
/******************************************************************/
/* create time stamps (for a test) */
/******************************************************************/
timeV.time = (LIGOTimeGPS *)LALMalloc(mObsCoh*sizeof(LIGOTimeGPS));
timeV.time[0].gpsSeconds = T0SEC;
timeV.time[0].gpsNanoSeconds = T0NSEC;
for(j=1; j<timeV.length; ++j){
timeV.time[j].gpsSeconds = timeV.time[j-1].gpsSeconds + TCOH + JUMPTIME;
timeV.time[j].gpsNanoSeconds = T0NSEC;
}
/******************************************************************/
/* compute detector velocity for those time stamps (for a test) */
/******************************************************************/
velV.data = (REAL8Cart3Coor *)LALMalloc(mObsCoh*sizeof(REAL8Cart3Coor));
velPar.edat = NULL;
{
EphemerisData *edat=NULL;
/* ephemeris info */
edat = (EphemerisData *)LALMalloc(sizeof(EphemerisData));
(*edat).ephiles.earthEphemeris = earthEphemeris;
(*edat).ephiles.sunEphemeris = sunEphemeris;
/* read in ephemeris data */
SUB( LALInitBarycenter( &status, edat), &status);
velPar.edat = edat;
for(j=0; j<velV.length; ++j){
velPar.startTime.gpsSeconds = timeV.time[j].gpsSeconds;
velPar.startTime.gpsNanoSeconds = timeV.time[j].gpsNanoSeconds;
SUB( LALAvgDetectorVel ( &status, vel, &velPar), &status );
velV.data[j].x= vel[0];
velV.data[j].y= vel[1];
velV.data[j].z= vel[2];
}
LALFree(edat->ephemE);
LALFree(edat->ephemS);
LALFree(edat);
}
/******************************************************************/
/******************************************************************/
/* create patch grid */
/******************************************************************/
SUB( LALHOUGHComputeNDSizePar( &status, &parSize, &parRes ), &status );
xSide = parSize.xSide;
ySide = parSize.ySide;
maxNBins = parSize.maxNBins;
maxNBorders = parSize.maxNBorders;
/* allocate memory based on xSide and ySide */
patch.xSide = xSide;
patch.ySide = ySide;
/* allocate memory based on xSide and ySide */
patch.xCoor = NULL;
patch.yCoor = NULL;
patch.xCoor = (REAL8 *)LALMalloc(xSide*sizeof(REAL8));
patch.yCoor = (REAL8 *)LALMalloc(ySide*sizeof(REAL8));
SUB( LALHOUGHFillPatchGrid( &status, &patch, &parSize ), &status );
/******************************************************************/
/******************************************************************/
/* memory allocation and settings */
/******************************************************************/
lutV.lut = (HOUGHptfLUT *)LALMalloc(mObsCoh*sizeof(HOUGHptfLUT));
pgV.pg = (HOUGHPeakGram *)LALMalloc(mObsCoh*sizeof(HOUGHPeakGram));
phmdVS.phmd =(HOUGHphmd *)LALMalloc(mObsCoh*NFSIZE*sizeof(HOUGHphmd));
freqInd.data = ( UINT8 *)LALMalloc(mObsCoh*sizeof(UINT8));
for(j=0; j<lutV.length; ++j){
lutV.lut[j].maxNBins = maxNBins;
lutV.lut[j].maxNBorders = maxNBorders;
lutV.lut[j].border =
(HOUGHBorder *)LALMalloc(maxNBorders*sizeof(HOUGHBorder));
lutV.lut[j].bin =
(HOUGHBin2Border *)LALMalloc(maxNBins*sizeof(HOUGHBin2Border));
}
for(j=0; j<phmdVS.length * phmdVS.nfSize; ++j){
phmdVS.phmd[j].maxNBorders = maxNBorders;
phmdVS.phmd[j].leftBorderP =
(HOUGHBorder **)LALMalloc(maxNBorders*sizeof(HOUGHBorder *));
phmdVS.phmd[j].rightBorderP =
(HOUGHBorder **)LALMalloc(maxNBorders*sizeof(HOUGHBorder *));
}
ht.xSide = xSide;
ht.ySide = ySide;
ht.map = NULL;
ht.map = (HoughTT *)LALMalloc(xSide*ySide*sizeof(HoughTT));
for(j=0; j<phmdVS.length * phmdVS.nfSize; ++j){
phmdVS.phmd[j].ySide = ySide;
phmdVS.phmd[j].firstColumn = NULL;
phmdVS.phmd[j].firstColumn = (UCHAR *)LALMalloc(ySide*sizeof(UCHAR));
}
for (j=0; j<lutV.length ; ++j){
for (i=0; i<maxNBorders; ++i){
lutV.lut[j].border[i].ySide = ySide;
lutV.lut[j].border[i].xPixel =
(COORType *)LALMalloc(ySide*sizeof(COORType));
}
}
/******************************************************************/
/******************************************************************/
/* create Peakgrams for testing */
/******************************************************************/
/* let us fix a source at a given position
(not at the center of the patch, but a pixel center) */
{
UINT2 xPos, yPos;
REAL8Cart2Coor sourceProjected;
REAL8UnitPolarCoor sourceRotated;
REAL8UnitPolarCoor skyPatchCenter;
REAL8UnitPolarCoor sourceLocation;
xPos = xSide/2;
yPos = ySide/2;
sourceProjected.x = patch.xCoor[xPos];
sourceProjected.y = patch.yCoor[yPos];
skyPatchCenter.alpha = parDem.skyPatch.alpha;
skyPatchCenter.delta = parDem.skyPatch.delta;
/* invert the stereographic projection for a point on the projected plane */
SUB( LALStereoInvProjectCart( &status, &sourceRotated, &sourceProjected ), &status );
/* undo roation in case the patch is not centered at the south pole */
SUB( LALInvRotatePolarU( &status, &sourceLocation, &sourceRotated, &skyPatchCenter ), &status );
Xx= cos(sourceLocation.delta)* cos(sourceLocation.alpha);
Xy= cos(sourceLocation.delta)* sin(sourceLocation.alpha);
Xz= sin(sourceLocation.delta);
}
for (j=0;j< mObsCoh; ++j) { /* create all the peakgrams */
REAL8 veldotX;
pgV.pg[j].deltaF = DF;
/*
* pgV.pg[j].fBinIni = f0Bin/2;
* pgV.pg[j].fBinFin = f0Bin*2;
*/
pgV.pg[j].fBinIni = f0Bin-maxNBins;
pgV.pg[j].fBinFin = f0Bin+3*maxNBins;
/* pgV.pg[j].length = 2; */
pgV.pg[j].length = 1;
pgV.pg[j].peak = NULL;
pgV.pg[j].peak = (INT4 *)LALMalloc( ( pgV.pg[j].length) * sizeof(INT4));
veldotX = Xx*velV.data[j].x + Xy*velV.data[j].y + Xz*velV.data[j].z;
/* fBin = [ f0Bin * ( 1 + veldotX ) ] */
pgV.pg[j].peak[0]= floor( f0Bin*(1.0+veldotX) +0.5) - pgV.pg[j].fBinIni;
/* pgV.pg[j].peak[1]= pgV.pg[j].peak[0]+2; */
}
/******************************************************************/
/******************************************************************/
/* create all the LUTs */
/******************************************************************/
for (j=0;j< mObsCoh;++j){ /* create all the LUTs */
parDem.veloC.x = velV.data[j].x;
parDem.veloC.y = velV.data[j].y;
parDem.veloC.z = velV.data[j].z;
/* calculate parameters needed for buiding the LUT */
SUB( LALNDHOUGHParamPLUT( &status, &parLut, &parSize, &parDem ), &status );
/* build the LUT */
SUB( LALHOUGHConstructPLUT( &status, &(lutV.lut[j]), &patch, &parLut ),
&status );
}
/******************************************************************/
/* starting the search (A very simple case for only 1 frequency) */
/******************************************************************/
/* build the set of PHMD starting at f0Bin*/
/******************************************************************/
fBin = f0Bin;
phmdVS.fBinMin = fBin;
SUB( LALHOUGHConstructSpacePHMD(&status, &phmdVS, &pgV, &lutV), &status );
/* shift the structure one frequency bin */
/* SUB( LALHOUGHupdateSpacePHMDup(&status, &phmdVS, &pgV, &lutV), &status );*/
/******************************************************************/
/* initializing the Hough map space */
/******************************************************************/
SUB( LALHOUGHInitializeHT( &status, &ht, &patch ), &status );
/******************************************************************/
/* construction of a total Hough map */
/******************************************************************/
for (j=0;j< mObsCoh;++j){
freqInd.data[j]= fBin;
}
SUB( LALHOUGHConstructHMT( &status, &ht, &freqInd, &phmdVS ), &status );
/******************************************************************/
/* printing the results into a particular file */
/* if the -o option was given, or into FILEOUT */
/******************************************************************/
if ( fname ) {
fp = fopen( fname, "w" );
} else {
fp = fopen( FILEOUT , "w" );
}
if ( !fp ){
ERROR( VALIDATION1_EFILE, VALIDATION1_MSGEFILE, 0 );
return VALIDATION1_EFILE;
}
for(k=ySide-1; k>=0; --k){
for(i=0;i<xSide;++i){
fprintf( fp ," %d", ht.map[k*xSide +i]);
fflush( fp );
}
fprintf( fp ," \n");
fflush( fp );
}
fclose( fp );
/******************************************************************/
/* Free memory and exit */
/******************************************************************/
for (j=0;j< mObsCoh;++j){
LALFree( pgV.pg[j].peak); /* All of them */
}
for (j=0; j<lutV.length ; ++j){
for (i=0; i<maxNBorders; ++i){
LALFree( lutV.lut[j].border[i].xPixel);
}
LALFree( lutV.lut[j].border);
LALFree( lutV.lut[j].bin);
}
for(j=0; j<phmdVS.length * phmdVS.nfSize; ++j){
LALFree( phmdVS.phmd[j].leftBorderP);
LALFree( phmdVS.phmd[j].rightBorderP);
LALFree( phmdVS.phmd[j].firstColumn);
}
LALFree(timeV.time);
LALFree(velV.data);
LALFree(lutV.lut);
LALFree(pgV.pg);
LALFree(phmdVS.phmd);
LALFree(freqInd.data);
LALFree(ht.map);
LALFree(patch.xCoor);
LALFree(patch.yCoor);
LALCheckMemoryLeaks();
INFO( VALIDATION1_MSGENORM );
return VALIDATION1_ENORM;
}
bool intersect_triangle(double orig[3], double dir[3],
double vert0[3], double vert1[3], double vert2[3],
double *t, double *u, double *v)
{
bool hit = true;
double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
double det,inv_det;
/* find vectors for two edges sharing vert0 */
SUB(edge1, vert1, vert0);
SUB(edge2, vert2, vert0);
/* begin calculating determinant - also used to calculate U parameter */
CROSS(pvec, dir, edge2);
/* if determinant is near zero, ray lies in plane of triangle */
det = DOT(edge1, pvec);
#ifdef TEST_CULL /* define TEST_CULL if culling is desired */
if (det < EPSILON)
hit = false;
/* calculate distance from vert0 to ray origin */
SUB(tvec, orig, vert0);
/* calculate U parameter and test bounds */
*u = DOT(tvec, pvec);
if (*u < 0.0 || *u > det)
hit = false;
/* prepare to test V parameter */
CROSS(qvec, tvec, edge1);
/* calculate V parameter and test bounds */
*v = DOT(dir, qvec);
if (*v < 0.0 || *u + *v > det)
hit = false;
/* calculate t, scale parameters, ray intersects triangle */
*t = DOT(edge2, qvec);
inv_det = 1.0 / det;
*t *= inv_det;
*u *= inv_det;
*v *= inv_det;
#else /* the non-culling branch */
if (det > -EPSILON && det < EPSILON)
hit = false;
inv_det = 1.0 / det;
/* calculate distance from vert0 to ray origin */
SUB(tvec, orig, vert0);
/* calculate U parameter and test bounds */
*u = DOT(tvec, pvec) * inv_det;
if (*u < 0.0 || *u > 1.0)
hit = false;
/* prepare to test V parameter */
CROSS(qvec, tvec, edge1);
/* calculate V parameter and test bounds */
*v = DOT(dir, qvec) * inv_det;
if (*v < 0.0 || *u + *v > 1.0)
hit = false;
/* calculate t, ray intersects triangle */
*t = DOT(edge2, qvec) * inv_det;
#endif
return hit;
}
int IsTriangleIntersectBox(float boxcenter[3], float boxhalf[3], float triverts[3][3])
{
/* use separating axis theorem to test overlap between triangle and box */
/* need to test for overlap in these directions: */
/* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
/* we do not even need to test these) */
/* 2) normal of the triangle */
/* 3) crossproduct(edge from tri, {x,y,z}-directin) */
/* this gives 3x3=9 more tests */
float v0[3],v1[3],v2[3];
float min,max,d,p0,p1,p2,rad,fex,fey,fez;
float normal[3],e0[3],e1[3],e2[3];
/* 1) first test overlap in the {x,y,z}-directions */
/* find min, max of the triangle each direction, and test for overlap in */
/* that direction -- this is equivalent to testing a minimal AABB around */
/* the triangle against the AABB */
/* test in X */
v0[X]=triverts[0][X]-boxcenter[X];
v1[X]=triverts[1][X]-boxcenter[X];
v2[X]=triverts[2][X]-boxcenter[X];
FINDMINMAX(v0[X],v1[X],v2[X],min,max);
if(min>boxhalf[X] || max<-boxhalf[X]) return 0;
/* test in Y */
v0[Y]=triverts[0][Y]-boxcenter[Y];
v1[Y]=triverts[1][Y]-boxcenter[Y];
v2[Y]=triverts[2][Y]-boxcenter[Y];
FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
if(min>boxhalf[Y] || max<-boxhalf[Y]) return 0;
/* test in Z */
v0[Z]=triverts[0][Z]-boxcenter[Z];
v1[Z]=triverts[1][Z]-boxcenter[Z];
v2[Z]=triverts[2][Z]-boxcenter[Z];
FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
if(min>boxhalf[Z] || max<-boxhalf[Z]) return 0;
/* 2) */
/* test if the box intersects the plane of the triangle */
/* compute plane equation of triangle: normal*x+d=0 */
SUB(e0,v1,v0); /* tri edge 0 */
SUB(e1,v2,v1); /* tri edge 1 */
CROSS(normal,e0,e1);
d=-DOT(normal,v0); /* plane eq: normal.x+d=0 */
if(!planeBoxOverlap(normal,d,boxhalf)) return 0;
/* compute the last triangle edge */
SUB(e2,v0,v2);
/* 3) */
fex = fabsf(e0[X]);
fey = fabsf(e0[Y]);
fez = fabsf(e0[Z]);
AXISTEST_X01(e0[Z], e0[Y], fez, fey);
AXISTEST_Y02(e0[Z], e0[X], fez, fex);
AXISTEST_Z12(e0[Y], e0[X], fey, fex);
fex = fabsf(e1[X]);
fey = fabsf(e1[Y]);
fez = fabsf(e1[Z]);
AXISTEST_X01(e1[Z], e1[Y], fez, fey);
AXISTEST_Y02(e1[Z], e1[X], fez, fex);
AXISTEST_Z0(e1[Y], e1[X], fey, fex);
fex = fabsf(e2[X]);
fey = fabsf(e2[Y]);
fez = fabsf(e2[Z]);
AXISTEST_X2(e2[Z], e2[Y], fez, fey);
AXISTEST_Y1(e2[Z], e2[X], fez, fex);
AXISTEST_Z12(e2[Y], e2[X], fey, fex);
return 1;
}
/*
* Fast quasi-reduction modulo p384 (FIPS 186-3 D.2.4)
*/
static int ecp_mod_p384(ttls_mpi *N)
{
INIT(384);
ADD(12); ADD(21); ADD(20);
SUB(23); NEXT; // A0
ADD(13); ADD(22); ADD(23);
SUB(12); SUB(20); NEXT; // A2
ADD(14); ADD(23);
SUB(13); SUB(21); NEXT; // A2
ADD(15); ADD(12); ADD(20); ADD(21);
SUB(14); SUB(22); SUB(23); NEXT; // A3
ADD(21); ADD(21); ADD(16); ADD(13); ADD(12); ADD(20); ADD(22);
SUB(15); SUB(23); SUB(23); NEXT; // A4
ADD(22); ADD(22); ADD(17); ADD(14); ADD(13); ADD(21); ADD(23);
SUB(16); NEXT; // A5
ADD(23); ADD(23); ADD(18); ADD(15); ADD(14); ADD(22);
SUB(17); NEXT; // A6
ADD(19); ADD(16); ADD(15); ADD(23);
SUB(18); NEXT; // A7
ADD(20); ADD(17); ADD(16);
SUB(19); NEXT; // A8
ADD(21); ADD(18); ADD(17);
SUB(20); NEXT; // A9
ADD(22); ADD(19); ADD(18);
SUB(21); NEXT; // A10
ADD(23); ADD(20); ADD(19);
SUB(22); LAST; // A11
cleanup:
return ret;
}
static image_t *
render_map (bsp_t *bsp)
{
long i = 0, j = 0, k = 0, x = 0;
dvertex_t *vertexlist, *vert1, *vert2;
dedge_t *edgelist;
dface_t *facelist;
int *ledges;
/* edge removal stuff */
struct edge_extra_t *edge_extra = NULL;
dvertex_t v0, v1, vect;
int area = 0, usearea;
float minX = 0.0, maxX = 0.0, minY = 0.0, maxY = 0.0, minZ = 0.0;
float maxZ = 0.0, midZ = 0.0, tempf = 0.0;
long Zoffset0 = 0, Zoffset1 = 0;
long Z_Xdir = 1, Z_Ydir = -1;
image_t *image;
int drawcol;
vertexlist = bsp->vertexes;
edgelist = bsp->edges;
facelist = bsp->faces;
ledges = bsp->surfedges;
/* Precalc stuff if we're removing edges - - - - - - - - - - - */
if (options.edgeremove) {
printf ("Precalc edge removal stuff...\n");
edge_extra = malloc (sizeof (struct edge_extra_t) * bsp->numedges);
if (edge_extra == NULL) {
fprintf (stderr, "Error allocating %ld bytes for extra edge info.",
(long) sizeof (struct edge_extra_t) * bsp->numedges);
exit (2);
}
/* initialize the array */
for (i = 0; i < bsp->numedges; i++) {
edge_extra[i].num_face_ref = 0;
for (j = 0; j < MAX_REF_FACES; j++) {
edge_extra[i].ref_faces[j] = -1;
}
}
for (i = 0; i < bsp->numfaces; i++) {
/* calculate the normal (cross product) */
/* starting edge: edgelist[ledges[facelist[i].firstedge]] */
/* number of edges: facelist[i].numedges; */
/* quick hack - just take the first 2 edges */
j = facelist[i].firstedge;
k = j;
vect.X = 0.0;
vect.Y = 0.0;
vect.Z = 0.0;
while (vect.X == 0.0 && vect.Y == 0.0 && vect.Z == 0.0
&& k < (facelist[i].numedges + j)) {
dedge_t *e1, *e2;
/* If the first 2 are parallel edges, go with the next one */
k++;
e1 = &edgelist[abs (ledges[j])];
e2 = &edgelist[abs (ledges[k])];
//FIXME verify directions
if (ledges[j] > 0) {
SUB (vertexlist[e1->v[0]], vertexlist[e1->v[1]], v0);
SUB (vertexlist[e2->v[0]], vertexlist[e2->v[1]], v1);
} else {
/* negative index, therefore walk in reverse order */
SUB (vertexlist[e1->v[1]], vertexlist[e1->v[0]], v0);
SUB (vertexlist[e2->v[1]], vertexlist[e2->v[0]], v1);
}
/* cross product */
CROSS (v0, v1, vect);
/* Okay, it's not the REAL area, but i'm lazy, and since a lot
of mapmakers use rectangles anyways... */
area = sqrt (DOT (v0, v0)) * sqrt (DOT (v1, v1));
} /* while */
/* reduce cross product to a unit vector */
tempf = sqrt (DOT (vect, vect));
if (tempf > 0.0) {
vect.X = vect.X / tempf;
vect.Y = vect.Y / tempf;
vect.Z = vect.Z / tempf;
} else {
vect.X = 0.0;
vect.Y = 0.0;
vect.Z = 0.0;
}
/* Now go put ref in all edges... */
for (j = 0; j < facelist[i].numedges; j++) {
edge_extra_t *e;
k = j + facelist[i].firstedge;
e = &edge_extra[abs (ledges[k])];
x = e->num_face_ref;
if (e->num_face_ref < MAX_REF_FACES) {
x++;
e->num_face_ref = x;
e->ref_faces[x - 1] = i;
e->ref_faces_normal[x - 1].X = vect.X;
e->ref_faces_normal[x - 1].Y = vect.Y;
e->ref_faces_normal[x - 1].Z = vect.Z;
e->ref_faces_area[x - 1] = area;
}
} /* for */
}
}
/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */
printf ("Collecting min/max\n");
/* Collect min and max */
for (i = 0; i < bsp->numvertexes; i++) {
/* Ugly hack - flip stuff around for different camera angles */
switch (options.camera_axis) {
case -1: /* -X -- (-y <--> +y, +x into screen,
-x out of screen; -z down, +z up) */
tempf = vertexlist[i].X;
vertexlist[i].X = vertexlist[i].Y;
vertexlist[i].Y = vertexlist[i].Z;
vertexlist[i].Z = -tempf;
break;
case 1: /* +X -- (+y <--> -y; -x into screen,
+x out of screen; -z down, +z up) */
tempf = vertexlist[i].X;
vertexlist[i].X = -vertexlist[i].Y;
vertexlist[i].Y = vertexlist[i].Z;
vertexlist[i].Z = tempf;
break;
case -2: /* -Y -- (+x <--> -x; -y out of screen,
+z up) */
vertexlist[i].X = -vertexlist[i].X;
tempf = vertexlist[i].Z;
vertexlist[i].Z = vertexlist[i].Y;
vertexlist[i].Y = tempf;;
break;
case 2: /* +Y -- (-x <--> +x; +y out of screen,
+z up) */
tempf = vertexlist[i].Z;
vertexlist[i].Z = -vertexlist[i].Y;
vertexlist[i].Y = tempf;;
break;
case -3: /* -Z -- negate X and Z (ie. 180 rotate
along Y axis) */
vertexlist[i].X = -vertexlist[i].X;
vertexlist[i].Z = -vertexlist[i].Z;
break;
case 3: /* +Z -- do nothing! */
default: /* do nothing! */
break;
}
/* flip Y for proper screen cords */
vertexlist[i].Y = -vertexlist[i].Y;
/* max and min */
if (i == 0) {
minX = vertexlist[i].X;
maxX = vertexlist[i].X;
minY = vertexlist[i].Y;
maxY = vertexlist[i].Y;
minZ = vertexlist[i].Z;
maxZ = vertexlist[i].Z;
} else {
minX = min (vertexlist[i].X, minX);
maxX = max (vertexlist[i].X, maxX);
minY = min (vertexlist[i].Y, minY);
maxY = max (vertexlist[i].Y, maxY);
minZ = min (vertexlist[i].Z, minZ);
maxZ = max (vertexlist[i].Z, maxZ);
}
}
if (options.z_pad == -1)
options.z_pad = (long) (maxZ - minZ) / (options.scaledown * Z_PAD_HACK);
midZ = (maxZ + minZ) / 2.0;
printf ("\n");
printf ("Bounds: X [%8.4f .. %8.4f] %8.4f\n", minX, maxX, (maxX - minX));
printf (" Y [%8.4f .. %8.4f] %8.4f\n", minY, maxY, (maxY - minY));
printf (" Z [%8.4f .. %8.4f] %8.4f - mid: %8.4f\n", minZ, maxZ,
(maxZ - minZ), midZ);
/* image array */
{
long width, height;
width = (maxX - minX + 1) / options.scaledown;
width += (options.image_pad + options.z_pad) * 2;
height = (maxY - minY + 1) / options.scaledown;
height += (options.image_pad + options.z_pad) * 2;
image = create_image (width, height);
}
/* Zoffset calculations */
switch (options.z_direction) {
case 0:
Z_Xdir = 0; /* up */
Z_Ydir = 1;
break;
default: /* unknown */
case 1: /* up & right */
Z_Xdir = 1;
Z_Ydir = -1;
break;
case 2: /* right */
Z_Xdir = 1;
Z_Ydir = 0;
break;
case 3: /* down & right */
Z_Xdir = 1;
Z_Ydir = 1;
break;
case 4: /* down */
Z_Xdir = 0;
Z_Ydir = 1;
break;
case 5: /* down & left */
Z_Xdir = -1;
Z_Ydir = 1;
break;
case 6: /* left */
Z_Xdir = -1;
Z_Ydir = 0;
break;
case 7: /* up & left */
Z_Xdir = -1;
Z_Ydir = -1;
break;
}
/* Plot edges on image */
fprintf (stderr, "Plotting edges...");
k = 0;
drawcol = (options.edgeremove) ? 64 : 32;
for (i = 0; i < bsp->numedges; i++) {
/* Do a check on this line ... see if we keep this line or not */
/* run through all referenced faces */
/* ICK ... do I want to check area of all faces? */
usearea = INT_MAX;
if (options.edgeremove) {
if (edge_extra[i].num_face_ref > 1) {
tempf = 1.0;
/* dot products of all referenced faces */
for (j = 0; j < edge_extra[i].num_face_ref - 1; j = j + 2) {
tempf =
tempf * (DOT (edge_extra[i].ref_faces_normal[j],
edge_extra[i].ref_faces_normal[j + 1]));
/* What is the smallest area this edge references? */
if (usearea > edge_extra[i].ref_faces_area[j])
usearea = edge_extra[i].ref_faces_area[j];
if (usearea > edge_extra[i].ref_faces_area[j + 1])
usearea = edge_extra[i].ref_faces_area[j + 1];
}
} else {
tempf = 0.0;
}
} else {
tempf = 0.0;
}
vert1 = &vertexlist[edgelist[i].v[0]];
vert2 = &vertexlist[edgelist[i].v[1]];
SUB (*vert1, *vert2, vect);
if (abs (tempf) < options.flat_threshold
&& usearea > options.area_threshold
&& sqrt (DOT (vect, vect)) > options.linelen_threshold) {
float offs0, offs1;
Zoffset0 = (options.z_pad * (vert1->Z - midZ) / (maxZ - minZ));
Zoffset1 = (options.z_pad * (vert2->Z - midZ) / (maxZ - minZ));
offs0 = options.image_pad + options.z_pad + (Zoffset0 * Z_Xdir);
offs1 = options.image_pad + options.z_pad + (Zoffset1 * Z_Ydir);
bresline (image, (vert1->X - minX) / options.scaledown + offs0,
(vert1->Y - minY) / options.scaledown + offs0,
(vert2->X - minX) / options.scaledown + offs1,
(vert2->Y - minY) / options.scaledown + offs1,
drawcol);
} else {
k++;
}
}
printf ("%d edges plotted", bsp->numedges);
if (options.edgeremove) {
printf (" (%ld edges removed)\n", k);
} else {
printf ("\n");
}
/* Little gradient */
for (i = 0; i <= 255; i++) {
// across from top left
plotpoint (image, i, 0, 255 - i);
// down from top left
plotpoint (image, 0, i, 255 - i);
// back from top right
plotpoint (image, image->width - i - 1, 0, 255 - i);
// down from top right
plotpoint (image, image->width - 1, i, 255 - i);
// back from bottom right
plotpoint (image, image->width - i - 1, image->height - 1, 255 - i);
// up from bottom right
plotpoint (image, image->width - 1, image->height - i - 1, 255 - i);
// across from bottom left
plotpoint (image, i, image->height - 1, 255 - i);
// up from bottom left
plotpoint (image, 0, image->height - i - 1, 255 - i);
}
/* Negate image if necessary */
if (options.negative_image) {
for (i = 0; i < image->height; i++) {
for (j = 0; j < image->width; j++) {
image->image[i * image->width + j] =
255 - image->image[i * image->width + j];
}
}
}
if (options.edgeremove) {
free (edge_extra);
}
return image;
}
int
main( int argc, char **argv )
{
int arg; /* command-line argument counter */
BOOLEAN xyz = 0; /* whether -x, -y, or -z options were given */
INT4 verbosity = 0; /* verbosity level */
REAL8 x_1 = 0.0, x_2 = 0.0, dx; /* range and increment in x */
REAL8 y_1 = 0.0, y_2 = 0.0, dy; /* range and increment in y */
REAL8 z_1 = 0.0, z_2 = 0.0, dz; /* range and increment in z */
INT4 nx = 1, ny = 1, nz = 1; /* number of steps in each direction */
INT4 i, j, k; /* index in each direction */
REAL8 x, y, z, ddx, ddy, ddz; /* position and error in each direction */
REAL8 ddr, ddmax = 0.0; /* overall and maximum position error */
static LALStatus stat; /* status structure */
EarthPosition earth; /* terrestrial coordinates */
/*******************************************************************
* PARSE ARGUMENTS (arg stores the current position) *
*******************************************************************/
arg = 1;
while ( arg < argc ) {
/* Parse range options. */
if ( !strcmp( argv[arg], "-x" ) ) {
if ( argc > arg + 3 ) {
arg++;
xyz = 1;
x_1 = atof( argv[arg++] );
x_2 = atof( argv[arg++] );
nx = atoi( argv[arg++] );
} else {
ERROR( GEOCENTRICGEODETICTESTC_EARG,
GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return GEOCENTRICGEODETICTESTC_EARG;
}
} else if ( !strcmp( argv[arg], "-y" ) ) {
if ( argc > arg + 3 ) {
arg++;
xyz = 1;
y_1 = atof( argv[arg++] );
y_2 = atof( argv[arg++] );
ny = atoi( argv[arg++] );
} else {
ERROR( GEOCENTRICGEODETICTESTC_EARG,
GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return GEOCENTRICGEODETICTESTC_EARG;
}
} else if ( !strcmp( argv[arg], "-z" ) ) {
if ( argc > arg + 3 ) {
arg++;
xyz = 1;
z_1 = atof( argv[arg++] );
z_2 = atof( argv[arg++] );
nz = atoi( argv[arg++] );
} else {
ERROR( GEOCENTRICGEODETICTESTC_EARG,
GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return GEOCENTRICGEODETICTESTC_EARG;
}
}
/* Parse verbosity option. */
else if ( !strcmp( argv[arg], "-v" ) ) {
if ( argc > arg + 1 ) {
arg++;
verbosity = atoi( argv[arg++] );
} else {
ERROR( GEOCENTRICGEODETICTESTC_EARG,
GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return GEOCENTRICGEODETICTESTC_EARG;
}
}
/* Parse debug level option. */
else if ( !strcmp( argv[arg], "-d" ) ) {
if ( argc > arg + 1 ) {
arg++;
} else {
ERROR( GEOCENTRICGEODETICTESTC_EARG,
GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return GEOCENTRICGEODETICTESTC_EARG;
}
}
/* Check for unrecognized options. */
else if ( argv[arg][0] == '-' ) {
ERROR( GEOCENTRICGEODETICTESTC_EARG,
GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
LALPrintError( USAGE, *argv );
return GEOCENTRICGEODETICTESTC_EARG;
}
} /* End of argument parsing loop. */
/*******************************************************************
* SET PARAMETERS *
*******************************************************************/
/* If none of -x, -y, -z were given, generate a random position. */
if ( !xyz ) {
REAL4 lon, coslat, sinlat, rad; /* polar coordinates */
RandomParams *rparams = NULL; /* pseudorandom sequence parameters */
SUB( LALCreateRandomParams( &stat, &rparams, 0 ), &stat );
SUB( LALUniformDeviate( &stat, &lon, rparams ), &stat );
lon *= LAL_TWOPI;
SUB( LALUniformDeviate( &stat, &sinlat, rparams ), &stat );
coslat = sqrt( 1.0 - sinlat*sinlat );
SUB( LALUniformDeviate( &stat, &rad, rparams ), &stat );
rad = 1.5*rad + 0.5;
x_1 = x_2 = rad*coslat*cos( lon );
y_1 = y_2 = rad*coslat*sin( lon );
z_1 = z_2 = rad*sinlat;
SUB( LALDestroyRandomParams( &stat, &rparams ), &stat );
}
/* Compute stepsizes. */
dx = dy = dz = 0.0;
if ( nx > 1 )
dx = ( x_2 - x_1 )/( nx - 1.0 );
if ( ny > 1 )
dy = ( y_2 - y_1 )/( ny - 1.0 );
if ( nz > 1 )
dz = ( z_2 - z_1 )/( nz - 1.0 );
/*******************************************************************
* PERFORM TEST *
*******************************************************************/
/* Loop over each direction. */
for ( i = 0; i < nx; i++ ) {
x = LAL_REARTH_SI*( x_1 + i*dx );
for ( j = 0; j < ny; j++ ) {
y = LAL_REARTH_SI*( y_1 + j*dy );
for ( k = 0; k < nz; k++ ) {
z = LAL_REARTH_SI*( z_1 + k*dz );
/* Do transformation. */
earth.x = x;
earth.y = y;
earth.z = z;
SUB( LALGeocentricToGeodetic( &stat, &earth ), &stat );
SUB( LALGeodeticToGeocentric( &stat, &earth ), &stat );
/* Compute difference. */
ddx = x - earth.x;
ddy = y - earth.y;
ddz = z - earth.z;
ddr = sqrt( ddx*ddx + ddy*ddy + ddz*ddz );
if ( ddr > ddmax )
ddmax = ddr;
if ( verbosity == 1 )
fprintf( stdout, "%+23.16e\n", ddr );
else if ( verbosity == 2 )
fprintf( stdout, "%+23.16e %+23.16e\n", earth.elevation, ddr );
else if ( verbosity == 3 )
fprintf( stdout, "%+23.16e %+23.16e %+23.16e %+23.16e\n",
x, y, z, ddr );
else if ( verbosity == 4 )
fprintf( stdout, "%+23.16e %+23.16e %+23.16e"
" %+23.16e %+23.16e %+23.16e %+23.16e\n", x, y, z,
earth.elevation, LAL_180_PI*earth.geodetic.latitude,
LAL_180_PI*earth.geodetic.longitude, ddr );
}
}
}
/* Print maximum. */
fprintf( stdout, "Maximum error: %.16em\n", ddmax );
if ( !xyz && ddmax > 1.0e-6 ) {
ERROR( GEOCENTRICGEODETICTESTC_ETEST,
GEOCENTRICGEODETICTESTC_MSGETEST, 0 );
return GEOCENTRICGEODETICTESTC_ETEST;
}
LALCheckMemoryLeaks();
INFO( GEOCENTRICGEODETICTESTC_MSGENORM );
return GEOCENTRICGEODETICTESTC_ENORM;
}
/*
* Fast quasi-reduction modulo p256 (FIPS 186-3 D.2.3)
*/
static int ecp_mod_p256(ttls_mpi *N)
{
INIT(256);
ADD( 8); ADD( 9);
SUB(11); SUB(12); SUB(13); SUB(14); NEXT; // A0
ADD( 9); ADD(10);
SUB(12); SUB(13); SUB(14); SUB(15); NEXT; // A1
ADD(10); ADD(11);
SUB(13); SUB(14); SUB(15); NEXT; // A2
ADD(11); ADD(11); ADD(12); ADD(12); ADD(13);
SUB(15); SUB( 8); SUB( 9); NEXT; // A3
ADD(12); ADD(12); ADD(13); ADD(13); ADD(14);
SUB( 9); SUB(10); NEXT; // A4
ADD(13); ADD(13); ADD(14); ADD(14); ADD(15);
SUB(10); SUB(11); NEXT; // A5
ADD(14); ADD(14); ADD(15); ADD(15); ADD(14); ADD(13);
SUB( 8); SUB( 9); NEXT; // A6
ADD(15); ADD(15); ADD(15); ADD(8);
SUB(10); SUB(11); SUB(12); SUB(13); LAST; // A7
cleanup:
return ret;
}
/* compute intersection of two convex polyhedrons */
TRI* cvi (double *va, int nva, double *pa, int npa, double *vb, int nvb, double *pb, int npb, CVIKIND kind, int *m, double **pv, int *nv)
{
double e [6], p [3], q [3], eps, d, *nl, *pt, *nn, *yy;
PFV *pfv, *v, *w, *z;
int i, j, k, n;
TRI *tri, *t;
/* initialize */
eps = GEOMETRIC_EPSILON;
tri = t = NULL;
pfv = NULL;
yy = NULL;
/* compute closest points */
d = gjk (va, nva, vb, nvb, p, q);
if (d > GEOMETRIC_EPSILON) { *m = 0; return NULL; }
/* push 'p' deeper inside only if regularized intersection is sought */
if (kind == REGULARIZED && !refine_point (pa, npa, pb, npb, p, &eps)) { *m = 0; return NULL; }
/* vertices extents for a later sanity check */
vertices_extents (va, nva, vb, nvb, eps, e);
/* translate base points of planes so that
* p = q = 0; compute new normals 'yy' */
ERRMEM (yy = malloc (sizeof (double [3]) * (npa+npb)));
for (i = 0, nl = pa, pt = pa + 3, nn = yy;
i < npa; i ++, nl += 6, pt += 6, nn += 3)
{
SUB (pt, p, q); /* q => translated point of current plane */
d = - DOT (nl, q); /* d => zero offset */
if (d > -GEOMETRIC_EPSILON) d = -eps; /* regularisation (tiny swelling) */
DIV (nl, -d, nn); /* <nn, x> <= 1 (yy stores vertices of polar polygon) */
}
for (i = 0, nl = pb, pt = pb + 3; i < npb;
i ++, nl += 6, pt += 6, nn += 3)
{
SUB (pt, p, q);
d = - DOT (nl, q);
if (d > -GEOMETRIC_EPSILON) d = -eps; /* regularisation (tiny swelling) */
DIV (nl, -d, nn);
}
/* compute and polarise convex
* hull of new normals 'yy' */
if (!(tri = hull (yy, npa+npb, &i))) goto error; /* tri = cv (polar (a) U polar (b)) */
if (!(pfv = TRI_Polarise (tri, i, &j))) goto error; /* pfv = polar (tri) => pfv = a * b */
/* normals in 'pfv' point to 'yy'; triangulate
* polar faces and set 'a' or 'b' flags */
/* count all face vertices */
for (k = n = 0; k < j; k ++)
{
v = &pfv [k]; n ++;
for (w = v->n; w != v; w = w->n) n ++;
}
/* there is (number of face vertices - 2) triangles per face,
* hence there is n - j * 2 triangles in total; vertex
* memory in 'pfv' is placed after n PFV items */
#if GEOMDEBUG
ASSERT_DEBUG (n - j*2 > 3, "Inconsitent polar faces => too few vertices in some faces");
#else
if (n - j*2 <= 3) goto error;
#endif
ERRMEM (tri = realloc (tri, sizeof (TRI) * (n-j*2) + sizeof (double [3]) * i)); /* allocate space for triangles and vertices */
pt = (double*) (tri + (n - j*2)); /* this is where output vertices begin */
nn = (double*) (pfv + n); /* this is where coords begin in 'pfv' block */
memcpy (pt, nn, sizeof (double [3]) * i); /* copy vertex data */
if (pv) *pv = pt;
if (nv) *nv = i;
/* shift point coords to the old 'zero' */
for (k = 0, nl = pt; k < i; k ++, nl += 3)
{
ADD (nl, p, nl); /* 'nl' used as a point */
if (nl[0] < e [0] || nl[1] < e [1] || nl[2] < e [2] ||
nl[0] > e [3] || nl[1] > e [4] || nl[2] > e [5])
{
#if GEOMDEBUG
printf ("CVI HAS GONE INSANE FOR THE INPUT:\n"), dump_input (va, nva, pa, npa, vb, nvb, pb, npb);
#endif
goto error;
}
}
for (k = 0, t = tri; k < j; k ++)
{
v = &pfv [k]; /* fixed vertex 'v' */
for (w = v->n, z = w->n; z != v; w = w->n, z = z->n, t ++) /* remaining vertices 'w' & 'z' */
{
COPY (v->nl, t->out); /* copy normal */
NORMALIZE (t->out);
t->ver [0] = pt + (v->coord - nn); /* map vertices */
t->ver [1] = pt + (w->coord - nn);
t->ver [2] = pt + (z->coord - nn);
t->flg = ((v->nl - yy) / 3) + 1; /* first 'npa' entries in 'yy' come from 'a' => positive 1-based index in 'a' */
if (t->flg > npa) t->flg = -(t->flg - npa); /* last 'npb' ones come from 'b' => negative 1-based index in 'b' */
}
}
goto done;
error:
if (tri) free (tri);
tri = t = NULL;
done:
free (yy);
free (pfv);
(*m) = (t - tri);
return tri;
}
int main() {
// distribute bodies in space (randomly)
for(int i=0; i<N; i++) {
B[i].m = (i < L)?1:0;
B[i].pos = triple_rand();
// B[i].pos = (position) { 0, -10 + 20*(i/2), -10 + 20*(i%2) }; // for debugging!
B[i].v = triple_zero();
}
// run simulation for M steps
#pragma omp parallel
for(int i=0; i<M; i++) {
// set forces to zero
#pragma omp for
for(int j=0; j<N; j++) {
F[j] = triple_zero();
}
// reset private copy
for(int j=0; j<N; j++) {
pF[j] = triple_zero();
}
// compute forces for each body (very naive)
#pragma omp for
for(int j=0; j<min(N,L); j++) {
for(int k=0; k<N; k++) {
if(j!=k) {
// comput distance vector
triple dist = SUB(B[k].pos, B[j].pos);
// compute absolute distance
double r = ABS(dist);
// compute strength of force (G = 1 (who cares))
// F = G * (m1 * m2) / r^2
double f = (B[j].m * B[k].m) / (r*r);
// compute current contribution to force
//force cur = MULS(NORM(dist), f);
double s = f / r;
force cur = MULS(dist,s);
// accumulate force
pF[j] = ADD(pF[j], cur);
}
}
}
// aggregate local data
#pragma omp critical
for(int j=0; j<N; j++) {
F[j] = ADD(pF[j],F[j]);
}
// apply forces
#pragma omp for
for(int j=0; j<min(N,L); j++) {
// update speed
// F = m * a
// a = F / m // m=1
// v' = v + a
B[j].v = ADD(B[j].v, DIVS(F[j], B[j].m));
// update position
// pos = pos + v * dt // dt = 1
B[j].pos = ADD(B[j].pos, B[j].v);
}
/* // debug print of positions and speed
for(int i=0; i<N; i++) {
printf("%2d - ", i);
triple_print(B[i].pos);
printf(" - ");
triple_print(B[i].v);
printf("\n");
}
printf("\n");
*/
}
// check result (impulse has to be zero)
impulse sum = triple_zero();
for(int i=0; i<N; i++) {
// impulse = m * v
sum = ADD(sum, MULS(B[i].v,B[i].m));
}
int success = EQ(sum, triple_zero());
printf("Verification: %s\n", ((success)?"OK":"ERR"));
if (!success) {
triple_print(sum); printf(" should be (0,0,0)\n");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
void foo()
{
int add = ADD(505);
int sub = SUB(525);
}
void decodeInstruction(instruction_t instruction,uint32_t *registro,flags_t *bandera, uint8_t *SRAM, uint16_t *codificacion, char **Flash)
{
int i;
*codificacion=0; // valor incial
// comparar el mnemonic con el nombre de cada una de las funciones, y asi ejecutar la adecuada
if( strcmp(instruction.mnemonic,"LDR") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
*codificacion=(13<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
instruction.op3_value<<=2;
if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
{
LDR(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,SRAM);
}
if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
{
*codificacion=(19<<11)+(instruction.op1_value<<8)+instruction.op3_value;
instruction.op3_value<<=2;
if(((*(registro+13)+instruction.op3_value)>=0x20000000)&&((*(registro+13)+instruction.op3_value)<0x40000000))
{
LDR(registro+instruction.op1_value,*(registro+13),instruction.op3_value,SRAM);
}
if((*(registro+13)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+13)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+13)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='P') && (instruction.op3_type=='#')) // label
{
*codificacion=(9<<11)+(instruction.op1_value<<8)+instruction.op3_value;
instruction.op3_value<<=2;
if(((*(registro+15)+instruction.op3_value)>=0x20000000)&&((*(registro+15)+instruction.op3_value)<0x40000000))
{
LDR(registro+instruction.op1_value,*(registro+15),instruction.op3_value,SRAM);
}
if((*(registro+15)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+15)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+15)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
*codificacion=(11<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
LDR(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
}
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"LDRB") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
*codificacion=(15<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
{
LDRB(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,SRAM);
}
if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
{
uint8_t data;
IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,&data,Read);
*(registro+instruction.op1_value)= (uint32_t)data;
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
*codificacion=(1<<14)+(7<<10)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
LDRB(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
uint8_t data;
IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,&data,Read);
*(registro+instruction.op1_value)=(uint32_t) data;
}
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"LDRH") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
*codificacion=(1<<15)+(1<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
instruction.op3_value<<=1;
if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
{
LDRH(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,SRAM);
}
if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
*codificacion=(5<<12)+(5<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
LDRH(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"LDRSB") ==0)
{
*codificacion=(5<<12)+(3<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
LDRSB(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"LDRSH") ==0)
{
*codificacion=(5<<12)+(7<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
LDRSH(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"STR") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
*codificacion=(3<<13)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
instruction.op3_value<<=2;
if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
{
STR(*(registro+instruction.op1_value),*(registro+instruction.op2_value),instruction.op3_value,SRAM);
}
if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Write);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
{
*codificacion=(9<<12)+(instruction.op1_value<<8)+instruction.op3_type;
instruction.op3_value<<=2;
if(((*(registro+13)+instruction.op3_value)>=0x20000000)&&((*(registro+13)+instruction.op3_value)<0x40000000))
{
STR(*(registro+instruction.op1_value),*(registro+13),instruction.op3_value,SRAM);
}
if((*(registro+13)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+13)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+13)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Write);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
*codificacion=(5<<12)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
STR(*(registro+instruction.op1_value),*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Write);
}
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"STRB") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
*codificacion=(7<<12)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
{
STRB(*(registro+instruction.op1_value),*(registro+instruction.op2_value),instruction.op3_value,SRAM);
}
if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
{
uint8_t data;
data=(uint8_t)(*(registro+instruction.op1_value));
IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,&data,Write);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
*codificacion=(21<<10)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
STRB(*(registro+instruction.op1_value),*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
uint8_t data;
data=(uint8_t)(*(registro+instruction.op1_value));
IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,&data,Write);
}
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"STRH") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
*codificacion=(1<<15)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
instruction.op3_value<<=1;
if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
{
STRH(*(registro+instruction.op1_value),*(registro+instruction.op2_value),instruction.op3_value,SRAM);
}
if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
{
}
if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Write);
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
*codificacion=(5<<12)+(1<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
{
STRH(*(registro+instruction.op1_value),*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
{
}
if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
{
//IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Write);
}
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"PUSH") ==0)
{
for(i=0;i<8;i++)
{
*codificacion+=(instruction.registers_list[i]<<i);
}
*codificacion+=(11<<12)+(1<<10)+(instruction.registers_list[14]<<8);
PUSH(registro,SRAM,&instruction.registers_list[0]);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"POP") ==0)
{
for(i=0;i<8;i++)
{
*codificacion+=(instruction.registers_list[i]<<i);
}
*codificacion=(11<<12)+(3<<10)+(instruction.registers_list[15]<<8);
POP(registro,SRAM,&instruction.registers_list[0]);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"ADCS") ==0)
{
*codificacion=(1<<14)+(5<<6)+(instruction.op2_value<<3)+instruction.op1_value;
ADCS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"ADD") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
{
ADD(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value));
*codificacion=(1<<14)+(1<<10)+(instruction.op2_value<<3)+((8&instruction.op1_value)<<4)+(7&instruction.op1_value);
}
if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
{
ADD(registro+instruction.op1_value,*(registro+13),instruction.op2_value);
*codificacion=(21<<11)+(instruction.op1_value<<8)+instruction.op3_value;
}
if((instruction.op1_type=='S') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
{
ADD(registro+13,*(registro+13),instruction.op3_value);
*codificacion=(11<<12)+instruction.op3_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='R'))
{
ADD(registro+instruction.op1_value,*(registro+13),*(registro+instruction.op3_value));
*codificacion=(1<<14)+(1<<10)+(13<<3)+((8&instruction.op1_value)<<4)+(7&instruction.op1_value);
}
if((instruction.op1_type=='S') && (instruction.op2_type=='R'))
{
ADD(registro+13,*(registro+13),*(registro+instruction.op2_value));
*codificacion=(1<<14)+(9<<7)+5+(instruction.op2_value<<3);
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"ADDS") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
ADDS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
*codificacion=(7<<10)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
{
ADDS(registro+instruction.op1_value,*(registro+instruction.op1_value),instruction.op2_value,bandera);
*codificacion=(3<<12)+(instruction.op1_value<<8)+instruction.op2_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
ADDS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
*codificacion=(3<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
registro[15]++;
}
// los parametros de las demas funciones aritmeticas, de desplazamiento y logicas son similares
if( strcmp(instruction.mnemonic,"ANDS") ==0)
{
*codificacion=(1<<14)+(instruction.op2_value<<3)+instruction.op1_value;
ANDS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"ASRS") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
{
ASRS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
*codificacion=(1<<14)+(1<<8)+(instruction.op2_value<<3)+instruction.op1_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
ASRS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
*codificacion=(1<<12)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"BICS") ==0)
{
*codificacion=(1<<14)+(14<<6)+(instruction.op2_value<<3)+instruction.op1_value;
BICS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"CMN") ==0)
{
*codificacion=(1<<14)+(11<6)+(instruction.op2_value<<3)+instruction.op1_value;
CMN(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera); // En diferencia a las demas funciones, se envian como parametros 2 valores
registro[15]++;
}
if( strcmp(instruction.mnemonic,"CMP") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
{
if(instruction.op1_value>=8)
{
CMP(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera); // En diferencia a las demas funciones, se envian como parametros 2 valores
*codificacion=(1<<14)+(5<<8)+(instruction.op2_value<<3)+(7&instruction.op1_value)+((8&instruction.op1_value)<<4);
}
else
{
CMP(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera); // En diferencia a las demas funciones, se envian como parametros 2 valores
*codificacion=(1<<14)+(10<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
}
if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
{
CMP(*(registro+instruction.op1_value),instruction.op2_value, bandera); // Como parametros se tienen el contenido de un registro y un valor
*codificacion=(5<<11)+(instruction.op1_value<<8)+instruction.op2_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"EORS") ==0)
{
*codificacion=(1<<14)+(1<<6)+(instruction.op2_value<<3)+instruction.op1_value;
EORS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value),bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"LSLS") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
{
LSLS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
*codificacion=(1<<14)+(1<<7)+(instruction.op2_value<<3)+instruction.op1_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
LSLS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
*codificacion=(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"LSRS") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
{
LSRS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
*codificacion=(1<<14)+(3<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
LSRS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
*codificacion=(1<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"MOV") ==0)
{
*codificacion=(1<<14)+(3<<9)+(instruction.op2_value<<3)+(7&instruction.op1_value)+((8&instruction.op1_value)<<4);
MOV(registro+instruction.op1_value,*(registro+instruction.op2_value)); // Envio como parametros una direccion y el contenido de un registro
registro[15]++;
}
if( strcmp(instruction.mnemonic,"MOVS") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
{
MOVS(registro+instruction.op1_value,instruction.op2_value, bandera);
*codificacion=(1<<13)+(instruction.op1_value<<8)+instruction.op2_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
{
MOVS(registro+instruction.op1_value,*(registro+instruction.op2_value),bandera);
*codificacion=(instruction.op2_value<<3)+instruction.op1_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"MULS") ==0)
{
*codificacion=(1<<14)+(13<<6)+(instruction.op2_value<<3)+instruction.op3_value;
MULS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"MVNS") ==0)
{
*codificacion=(1<<14)+(15<<6)+(instruction.op2_value<<3)+instruction.op1_value;
RSBS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"NOP") ==0)
{
*codificacion=(11<<12)+(15<<8);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"ORRS") ==0)
{
*codificacion=(1<<14)+(3<<8)+(instruction.op2_value<<3)+instruction.op1_value;
ORRS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"REV") ==0)
{
*codificacion=(11<<12)+(5<<9)+(instruction.op2_value<<3)+instruction.op1_value;
REV(registro+instruction.op1_value, *(registro+instruction.op2_value));
registro[15]++;
}
if( strcmp(instruction.mnemonic,"REV16") ==0)
{
*codificacion=(11<<12)+(5<<9)+(1<<6)+(instruction.op2_value<<3)+instruction.op1_value;
REV16(registro+instruction.op1_value,*(registro+instruction.op2_value));
registro[15]++;
}
if( strcmp(instruction.mnemonic,"REVSH") ==0)
{
*codificacion=(11<<12)+(5<<9)+(3<<6)+(instruction.op2_value<<3)+instruction.op1_value;
REVSH(registro+instruction.op1_value,*(registro+instruction.op2_value));
registro[15]++;
}
if( strcmp(instruction.mnemonic,"RORS") ==0)
{
*codificacion=(1<<14)+(7<<6)+(instruction.op2_value<<3)+instruction.op1_value;
RORS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"RSBS") ==0)
{
*codificacion=(1<<14)+(9<<6)+(instruction.op2_value<<3)+instruction.op1_value;
RSBS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"SBCS") ==0)
{
*codificacion=(1<<14)+(3<<7)+(instruction.op2_value<<3)+instruction.op1_value;
SBCS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
registro[15]++;
}
if( strcmp(instruction.mnemonic,"SUB") ==0)
{
if((instruction.op1_type=='S') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
{
SUB(registro+13,*(registro+13),instruction.op3_value);
*codificacion=(11<<12)+(1<<7)+instruction.op3_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"SUBS") ==0)
{
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
{
SUBS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
*codificacion=(15<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
{
SUBS(registro+instruction.op1_value,*(registro+instruction.op1_value),instruction.op2_value,bandera);
*codificacion=(7<<11)+(instruction.op1_value<<8)+instruction.op2_value;
}
if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
{
SUBS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
*codificacion=(13<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
}
registro[15]++;
}
if( strcmp(instruction.mnemonic,"TST") ==0)
{
*codificacion=(1<<14)+(1<<9)+(instruction.op2_value<<3)+instruction.op1_value;
TST(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera); // Como parametros se tienen el contenido de un registro y un valor
registro[15]++;
}
// Las siguientes funciones, son funciones de saltos
if( strcmp(instruction.mnemonic,"B") ==0)
{
*codificacion=(7<<13)+instruction.op1_value;
B(registro,instruction.op1_value); // Envio como parametroa la direccion de registro y el valor del salto
}
if( strcmp(instruction.mnemonic,"BL") ==0)
{
*codificacion=(31<<11)+(2047&instruction.op1_value);
BL(registro,instruction.op1_value); // Envio como parametroa la direccion de registro y el valor del salto
}
if( strcmp(instruction.mnemonic,"BLX") ==0)
{
*codificacion=(1<<14)+(15<<7)+(instruction.op1_value<<3);
BLX(registro,*(registro+instruction.op1_value)); // Envio como parametroa la direccion de registro y el contenido de un registro
}
if( strcmp(instruction.mnemonic,"BX") ==0)
{
*codificacion=(1<<14)+(14<<7)+(instruction.op1_value<<3);
if(instruction.op1_type=='L') // Sucede cuando
{
BX(registro,registro[14]); // PC=LR
}
if(instruction.op1_type=='R') // Sucede cuando se tiene como parametro un registro diferente a LR
{
BX(registro,*(registro+instruction.op1_value));
}
}
if( strcmp(instruction.mnemonic,"BEQ") ==0)
{
*codificacion=(13<<12)+instruction.op1_value;
BEQ(registro,instruction.op1_value,*bandera); // Envio como parametros la direccion de registro, el valor del salto y las banderas
}
// Todas las siguientes funciones de salto tienen los mismos parametro que BEQ
if( strcmp(instruction.mnemonic,"BNE") ==0)
{
*codificacion=(13<<12)+(1<<8)+instruction.op1_value;
BNE(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BCS") ==0)
{
*codificacion=(13<<12)+(2<<8)+instruction.op1_value;
BCS(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BCC") ==0)
{
*codificacion=(13<<12)+(3<<8)+instruction.op1_value;
BCC(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BMI") ==0)
{
*codificacion=(13<<12)+(4<<8)+instruction.op1_value;
BMI(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BPL") ==0)
{
*codificacion=(13<<12)+(5<<8)+instruction.op1_value;
BPL(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BVS") ==0)
{
*codificacion=(13<<12)+(6<<8)+instruction.op1_value;
BVS(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BVC") ==0)
{
*codificacion=(13<<12)+(7<<8)+instruction.op1_value;
BVC(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BHI") ==0)
{
*codificacion=(13<<12)+(8<<8)+instruction.op1_value;
BHI(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BLS") ==0)
{
*codificacion=(13<<12)+(9<<8)+instruction.op1_value;
BLS(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BGE") ==0)
{
*codificacion=(13<<12)+(10<<8)+instruction.op1_value;
BGE(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BLT") ==0)
{
*codificacion=(13<<12)+(11<<8)+instruction.op1_value;
BLT(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BGT") ==0)
{
*codificacion=(13<<12)+(12<<8)+instruction.op1_value;
BGT(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BLE") ==0)
{
*codificacion=(13<<12)+(13<<8)+instruction.op1_value;
BLE(registro,instruction.op1_value,*bandera);
}
if( strcmp(instruction.mnemonic,"BAL") ==0)
{
*codificacion=(13<<12)+(14<<8)+instruction.op1_value;
BAL(registro,instruction.op1_value,*bandera);
}
}
double ppf_sub_callback(double x, double y)
{
return(SUB(x,y));
}
void GGLAssembler::downshift(
pixel_t& d, int component, component_t s, const reg_t& dither)
{
const needs_t& needs = mBuilderContext.needs;
Scratch scratches(registerFile());
int sh = s.h;
int sl = s.l;
int maskHiBits = (sh!=32) ? ((s.flags & CLEAR_HI)?1:0) : 0;
int maskLoBits = (sl!=0) ? ((s.flags & CLEAR_LO)?1:0) : 0;
int sbits = sh - sl;
int dh = d.format.c[component].h;
int dl = d.format.c[component].l;
int dbits = dh - dl;
int dithering = 0;
ALOGE_IF(sbits<dbits, "sbits (%d) < dbits (%d) in downshift", sbits, dbits);
if (sbits>dbits) {
// see if we need to dither
dithering = mDithering;
}
int ireg = d.reg;
if (!(d.flags & FIRST)) {
if (s.flags & CORRUPTIBLE) {
ireg = s.reg;
} else {
ireg = scratches.obtain();
}
}
d.flags &= ~FIRST;
if (maskHiBits) {
// we need to mask the high bits (and possibly the lowbits too)
// and we might be able to use immediate mask.
if (!dithering) {
// we don't do this if we only have maskLoBits because we can
// do it more efficiently below (in the case where dl=0)
const int offset = sh - dbits;
if (dbits<=8 && offset >= 0) {
const uint32_t mask = ((1<<dbits)-1) << offset;
if (isValidImmediate(mask) || isValidImmediate(~mask)) {
build_and_immediate(ireg, s.reg, mask, 32);
sl = offset;
s.reg = ireg;
sbits = dbits;
maskLoBits = maskHiBits = 0;
}
}
} else {
// in the dithering case though, we need to preserve the lower bits
const uint32_t mask = ((1<<sbits)-1) << sl;
if (isValidImmediate(mask) || isValidImmediate(~mask)) {
build_and_immediate(ireg, s.reg, mask, 32);
s.reg = ireg;
maskLoBits = maskHiBits = 0;
}
}
}
// XXX: we could special case (maskHiBits & !maskLoBits)
// like we do for maskLoBits below, but it happens very rarely
// that we have maskHiBits only and the conditions necessary to lead
// to better code (like doing d |= s << 24)
if (maskHiBits) {
MOV(AL, 0, ireg, reg_imm(s.reg, LSL, 32-sh));
sl += 32-sh;
sh = 32;
s.reg = ireg;
maskHiBits = 0;
}
// Downsampling should be performed as follows:
// V * ((1<<dbits)-1) / ((1<<sbits)-1)
// V * [(1<<dbits)/((1<<sbits)-1) - 1/((1<<sbits)-1)]
// V * [1/((1<<sbits)-1)>>dbits - 1/((1<<sbits)-1)]
// V/((1<<(sbits-dbits))-(1>>dbits)) - (V>>sbits)/((1<<sbits)-1)>>sbits
// V/((1<<(sbits-dbits))-(1>>dbits)) - (V>>sbits)/(1-(1>>sbits))
//
// By approximating (1>>dbits) and (1>>sbits) to 0:
//
// V>>(sbits-dbits) - V>>sbits
//
// A good approximation is V>>(sbits-dbits),
// but better one (needed for dithering) is:
//
// (V>>(sbits-dbits)<<sbits - V)>>sbits
// (V<<dbits - V)>>sbits
// (V - V>>dbits)>>(sbits-dbits)
// Dithering is done here
if (dithering) {
comment("dithering");
if (sl) {
MOV(AL, 0, ireg, reg_imm(s.reg, LSR, sl));
sh -= sl;
sl = 0;
s.reg = ireg;
}
// scaling (V-V>>dbits)
SUB(AL, 0, ireg, s.reg, reg_imm(s.reg, LSR, dbits));
const int shift = (GGL_DITHER_BITS - (sbits-dbits));
if (shift>0) ADD(AL, 0, ireg, ireg, reg_imm(dither.reg, LSR, shift));
else if (shift<0) ADD(AL, 0, ireg, ireg, reg_imm(dither.reg, LSL,-shift));
else ADD(AL, 0, ireg, ireg, dither.reg);
s.reg = ireg;
}
if ((maskLoBits|dithering) && (sh > dbits)) {
int shift = sh-dbits;
if (dl) {
MOV(AL, 0, ireg, reg_imm(s.reg, LSR, shift));
if (ireg == d.reg) {
MOV(AL, 0, d.reg, reg_imm(ireg, LSL, dl));
} else {
ORR(AL, 0, d.reg, d.reg, reg_imm(ireg, LSL, dl));
}
} else {
if (ireg == d.reg) {
MOV(AL, 0, d.reg, reg_imm(s.reg, LSR, shift));
} else {
ORR(AL, 0, d.reg, d.reg, reg_imm(s.reg, LSR, shift));
}
}
} else {
int shift = sh-dh;
if (shift>0) {
if (ireg == d.reg) {
MOV(AL, 0, d.reg, reg_imm(s.reg, LSR, shift));
} else {
ORR(AL, 0, d.reg, d.reg, reg_imm(s.reg, LSR, shift));
}
} else if (shift<0) {
if (ireg == d.reg) {
MOV(AL, 0, d.reg, reg_imm(s.reg, LSL, -shift));
} else {
ORR(AL, 0, d.reg, d.reg, reg_imm(s.reg, LSL, -shift));
}
} else {
if (ireg == d.reg) {
if (s.reg != d.reg) {
MOV(AL, 0, d.reg, s.reg);
}
} else {
ORR(AL, 0, d.reg, d.reg, s.reg);
}
}
}
}
static void
read_PK_char(struct font *fontp, wide_ubyte ch)
{
int i, j;
int n;
int row_bit_pos;
Boolean paint_switch;
BMUNIT *cp;
struct glyph *g;
FILE *fp = fontp->file;
long fpwidth;
BMUNIT word = 0;
int word_weight, bytes_wide;
int rows_left, h_bit, count;
g = &fontp->glyph[ch];
PK_flag_byte = g->x2;
PK_dyn_f = PK_flag_byte >> 4;
paint_switch = ((PK_flag_byte & 8) != 0);
PK_flag_byte &= 0x7;
if (PK_flag_byte == 7)
n = 4;
else if (PK_flag_byte > 3)
n = 2;
else
n = 1;
if (debug & DBG_PK)
Printf("loading pk char %d, char type %d ", ch, n);
/*
* now read rest of character preamble
*/
if (n != 4)
fpwidth = num(fp, 3);
else {
fpwidth = sfour(fp);
(void)four(fp); /* horizontal escapement */
}
(void)num(fp, n); /* vertical escapement */
{
unsigned long w, h;
w = num(fp, n);
h = num(fp, n);
if (w > 0x7fff || h > 0x7fff)
oops("Character %d too large in file %s", ch, fontp->fontname);
g->bitmap.w = w;
g->bitmap.h = h;
}
g->x = snum(fp, n);
g->y = snum(fp, n);
g->dvi_adv = fontp->dimconv * fpwidth;
if (debug & DBG_PK) {
if (g->bitmap.w != 0)
Printf(", size=%dx%d, dvi_adv=%ld", g->bitmap.w, g->bitmap.h,
g->dvi_adv);
Putchar('\n');
}
alloc_bitmap(&g->bitmap);
cp = (BMUNIT *) g->bitmap.bits;
/*
* read character data into *cp
*/
bytes_wide = ROUNDUP((int)g->bitmap.w, BMBITS) * BMBYTES;
PK_bitpos = -1;
if (PK_dyn_f == 14) { /* get raster by bits */
bzero(g->bitmap.bits, (int)g->bitmap.h * bytes_wide);
for (i = 0; i < (int)g->bitmap.h; i++) { /* get all rows */
cp = ADD(g->bitmap.bits, i * bytes_wide);
#ifndef WORDS_BIGENDIAN
row_bit_pos = -1;
#else
row_bit_pos = BMBITS;
#endif
for (j = 0; j < (int)g->bitmap.w; j++) { /* get one row */
if (--PK_bitpos < 0) {
word = one(fp);
PK_bitpos = 7;
}
#ifndef WORDS_BIGENDIAN
if (++row_bit_pos >= BMBITS) {
cp++;
row_bit_pos = 0;
}
#else
if (--row_bit_pos < 0) {
cp++;
row_bit_pos = BMBITS - 1;
}
#endif
if (word & (1 << PK_bitpos))
*cp |= 1 << row_bit_pos;
}
}
}
else { /* get packed raster */
rows_left = g->bitmap.h;
h_bit = g->bitmap.w;
PK_repeat_count = 0;
word_weight = BMBITS;
word = 0;
while (rows_left > 0) {
count = PK_packed_num(fp);
while (count > 0) {
if (count < word_weight && count < h_bit) {
#ifndef WORDS_BIGENDIAN
if (paint_switch)
word |= bit_masks[count] << (BMBITS - word_weight);
#endif
h_bit -= count;
word_weight -= count;
#ifdef WORDS_BIGENDIAN
if (paint_switch)
word |= bit_masks[count] << word_weight;
#endif
count = 0;
}
else if (count >= h_bit && h_bit <= word_weight) {
if (paint_switch)
word |= bit_masks[h_bit] <<
#ifndef WORDS_BIGENDIAN
(BMBITS - word_weight);
#else
(word_weight - h_bit);
#endif
*cp++ = word;
/* "output" row(s) */
for (i = PK_repeat_count * bytes_wide / BMBYTES; i > 0; --i) {
*cp = *SUB(cp, bytes_wide);
++cp;
}
rows_left -= PK_repeat_count + 1;
PK_repeat_count = 0;
word = 0;
word_weight = BMBITS;
count -= h_bit;
h_bit = g->bitmap.w;
}
else {
if (paint_switch)
#ifndef WORDS_BIGENDIAN
word |= bit_masks[word_weight] <<
(BMBITS - word_weight);
#else
word |= bit_masks[word_weight];
#endif
*cp++ = word;
word = 0;
count -= word_weight;
h_bit -= word_weight;
word_weight = BMBITS;
}
}
paint_switch = 1 - paint_switch;
}
if (cp != ((BMUNIT *) (g->bitmap.bits + bytes_wide * g->bitmap.h)))
oops("Wrong number of bits stored: char. %d, font %s", ch,
fontp->fontname);
if (rows_left != 0 || h_bit != g->bitmap.w)
oops("Bad pk file (%s), too many bits", fontp->fontname);
}
}
Elem & Machine::execute(std::ostream &out)
{
Instruction *command;
std::shared_ptr<Elem> command_ptr;
Elem *ADD(new Instruction("ADD"));
Elem *MUL(new Instruction("MUL"));
Elem *SUB(new Instruction("SUB"));
Elem *DIV(new Instruction("DIV"));
Elem *REM(new Instruction("REM"));
Elem *EQ(new Instruction("EQ"));
Elem *LEQ(new Instruction("LEQ"));
Elem *SEL(new Instruction("SEL"));
Elem *LD(new Instruction("LD"));
Elem *LDC(new Instruction("LDC"));
Elem *LDF(new Instruction("LDF"));
Elem *CAR(new Instruction("CAR"));
Elem *CDR(new Instruction("CDR"));
Elem *CONS(new Instruction("CONS"));
Elem *NIL(new Instruction("NIL"));
Elem *DUM(new Instruction("DUM"));
Elem *AP(new Instruction("AP"));
Elem *RAP(new Instruction("RAP"));
Elem *RTN(new Instruction("RTN"));
Elem *JOIN(new Instruction("JOIN"));
Elem *STOP(new Instruction("STOP"));
while (!C->empty())
{
if (out != 0x0)
{
print_S(out);
print_E(out);
print_C(out);
out << std::endl;
}
command_ptr = C->pop_ret();
command = dynamic_cast<Instruction*>(&*command_ptr);
if (command == nullptr) throw Exception("Execute", "FatalError");
if (*command == *ADD) this->ADD();
else if (*command == *MUL) this->MUL();
else if (*command == *SUB) this->SUB();
else if (*command == *DIV) this->DIV();
else if (*command == *REM) this->REM();
else if (*command == *EQ) this->EQ();
else if (*command == *LEQ) this->LEQ();
else if (*command == *SEL) this->SEL();
else if (*command == *LD) this->LD();
else if (*command == *LDC) this->LDC();
else if (*command == *LDF) this->LDF();
else if (*command == *CAR) this->CAR();
else if (*command == *CDR) this->CDR();
else if (*command == *CONS) this->CONS();
else if (*command == *NIL) this->NIL();
else if (*command == *DUM) this->DUM();
else if (*command == *AP) this->AP();
else if (*command == *RAP) this->RAP();
else if (*command == *RTN) this->RTN();
else if (*command == *JOIN) this->JOIN();
else if (*command == *STOP) { return (*(this->STOP()));}
else throw Exception("Execute", "Expected 'instruction' but greeted constant.");
}
throw Exception("Execute", "FatalError");
}
int triBoxOverlap(double boxcenter[3], double boxhalfsize[3], double triverts[3][3])
{
/* use separating axis theorem to test overlap between triangle and box */
/* need to test for overlap in these directions: */
/* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
/* we do not even need to test these) */
/* 2) normal of the triangle */
/* 3) crossproduct(edge from tri, {x,y,z}-directin) */
/* this gives 3x3=9 more tests */
double v0[3], v1[3], v2[3];
// double axis[3];
double min, max, p0, p1, p2, rad, fex, fey, fez;
double normal[3], e0[3], e1[3], e2[3];
/* This is the fastest branch on Sun */
/* move everything so that the boxcenter is in (0,0,0) */
SUB(v0, triverts[0], boxcenter);
SUB(v1, triverts[1], boxcenter);
SUB(v2, triverts[2], boxcenter);
/* compute triangle edges */
SUB(e0, v1, v0); /* tri edge 0 */
SUB(e1, v2, v1); /* tri edge 1 */
SUB(e2, v0, v2); /* tri edge 2 */
/* Bullet 3: */
/* test the 9 tests first (this was faster) */
fex = fabs(e0[X]);
fey = fabs(e0[Y]);
fez = fabs(e0[Z]);
AXISTEST_X01(e0[Z], e0[Y], fez, fey);
AXISTEST_Y02(e0[Z], e0[X], fez, fex);
AXISTEST_Z12(e0[Y], e0[X], fey, fex);
fex = fabs(e1[X]);
fey = fabs(e1[Y]);
fez = fabs(e1[Z]);
AXISTEST_X01(e1[Z], e1[Y], fez, fey);
AXISTEST_Y02(e1[Z], e1[X], fez, fex);
AXISTEST_Z0(e1[Y], e1[X], fey, fex);
fex = fabs(e2[X]);
fey = fabs(e2[Y]);
fez = fabs(e2[Z]);
AXISTEST_X2(e2[Z], e2[Y], fez, fey);
AXISTEST_Y1(e2[Z], e2[X], fez, fex);
AXISTEST_Z12(e2[Y], e2[X], fey, fex);
/* Bullet 1: */
/* first test overlap in the {x,y,z}-directions */
/* find min, max of the triangle each direction, and test for overlap in */
/* that direction -- this is equivalent to testing a minimal AABB around */
/* the triangle against the AABB */
/* test in X-direction */
FINDMINMAX(v0[X], v1[X], v2[X], min, max);
if (min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0;
/* test in Y-direction */
FINDMINMAX(v0[Y], v1[Y], v2[Y], min, max);
if (min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0;
/* test in Z-direction */
FINDMINMAX(v0[Z], v1[Z], v2[Z], min, max);
if (min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0;
/* Bullet 2: */
/* test if the box intersects the plane of the triangle */
/* compute plane equation of triangle: normal*x+d=0 */
CROSS(normal, e0, e1);
if (!planeBoxOverlap(normal, v0, boxhalfsize)) return 0;
return 1; /* box and triangle overlaps */
}
EAPI double
ecore_animator_pos_map(double pos,
Ecore_Pos_Map map,
double v1,
double v2)
{
/* purely functional - locking not required */
if (pos > 1.0) pos = 1.0;
else if (pos < 0.0)
pos = 0.0;
switch (map)
{
case ECORE_POS_MAP_LINEAR:
return pos;
case ECORE_POS_MAP_ACCELERATE:
/* pos = 1 - sin(Pi / 2 + pos * Pi / 2); */
pos = DBL_TO(SUB(INT_FROM(1), SIN(ADD((EINA_F32P32_PI >> 1), MUL(DBL_FROM(pos), (EINA_F32P32_PI >> 1))))));
return pos;
case ECORE_POS_MAP_DECELERATE:
/* pos = sin(pos * Pi / 2); */
pos = DBL_TO(SIN(MUL(DBL_FROM(pos), (EINA_F32P32_PI >> 1))));
return pos;
case ECORE_POS_MAP_SINUSOIDAL:
/* pos = (1 - cos(pos * Pi)) / 2 */
pos = DBL_TO((SUB(INT_FROM(1), COS(MUL(DBL_FROM(pos), EINA_F32P32_PI)))) >> 1);
return pos;
case ECORE_POS_MAP_ACCELERATE_FACTOR:
pos = _pos_map_accel_factor(pos, v1);
return pos;
case ECORE_POS_MAP_DECELERATE_FACTOR:
pos = 1.0 - _pos_map_accel_factor(1.0 - pos, v1);
return pos;
case ECORE_POS_MAP_SINUSOIDAL_FACTOR:
if (pos < 0.5) pos = _pos_map_accel_factor(pos * 2.0, v1) / 2.0;
else pos = 1.0 - (_pos_map_accel_factor((1.0 - pos) * 2.0, v1) / 2.0);
return pos;
case ECORE_POS_MAP_DIVISOR_INTERP:
pos = _pos_map_pow(pos, v1, (int)v2);
return pos;
case ECORE_POS_MAP_BOUNCE:
pos = _pos_map_spring(pos, (int)v2, v1);
if (pos < 0.0) pos = -pos;
pos = 1.0 - pos;
return pos;
case ECORE_POS_MAP_SPRING:
pos = 1.0 - _pos_map_spring(pos, (int)v2, v1);
return pos;
default:
return pos;
}
return pos;
}
long t_c_intersection(Triangle3 t)
{
long v1_test,v2_test,v3_test;
float d,denom;
Point3 vect12,vect13,norm;
Point3 hitpp,hitpn,hitnp,hitnn;
/* First compare all three vertexes with all six face-planes */
/* If any vertex is inside the cube, return immediately! */
if ((v1_test = face_plane(t.v1)) == INSIDE) return(INSIDE);
if ((v2_test = face_plane(t.v2)) == INSIDE) return(INSIDE);
if ((v3_test = face_plane(t.v3)) == INSIDE) return(INSIDE);
/* If all three vertexes were outside of one or more face-planes, */
/* return immediately with a trivial rejection! */
if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);
/* Now do the same trivial rejection test for the 12 edge planes */
v1_test |= bevel_2d(t.v1) << 8;
v2_test |= bevel_2d(t.v2) << 8;
v3_test |= bevel_2d(t.v3) << 8;
if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);
/* Now do the same trivial rejection test for the 8 corner planes */
v1_test |= bevel_3d(t.v1) << 24;
v2_test |= bevel_3d(t.v2) << 24;
v3_test |= bevel_3d(t.v3) << 24;
if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);
/* If vertex 1 and 2, as a pair, cannot be trivially rejected */
/* by the above tests, then see if the v1-->v2 triangle edge */
/* intersects the cube. Do the same for v1-->v3 and v2-->v3. */
/* Pass to the intersection algorithm the "OR" of the outcode */
/* bits, so that only those cube faces which are spanned by */
/* each triangle edge need be tested. */
if ((v1_test & v2_test) == 0)
if (check_line(t.v1,t.v2,v1_test|v2_test) == INSIDE) return(INSIDE);
if ((v1_test & v3_test) == 0)
if (check_line(t.v1,t.v3,v1_test|v3_test) == INSIDE) return(INSIDE);
if ((v2_test & v3_test) == 0)
if (check_line(t.v2,t.v3,v2_test|v3_test) == INSIDE) return(INSIDE);
/* By now, we know that the triangle is not off to any side, */
/* and that its sides do not penetrate the cube. We must now */
/* test for the cube intersecting the interior of the triangle. */
/* We do this by looking for intersections between the cube */
/* diagonals and the triangle...first finding the intersection */
/* of the four diagonals with the plane of the triangle, and */
/* then if that intersection is inside the cube, pursuing */
/* whether the intersection point is inside the triangle itself. */
/* To find plane of the triangle, first perform crossproduct on */
/* two triangle side vectors to compute the normal vector. */
SUB(t.v1,t.v2,vect12);
SUB(t.v1,t.v3,vect13);
CROSS(vect12,vect13,norm)
/* The normal vector "norm" X,Y,Z components are the coefficients */
/* of the triangles AX + BY + CZ + D = 0 plane equation. If we */
/* solve the plane equation for X=Y=Z (a diagonal), we get */
/* -D/(A+B+C) as a metric of the distance from cube center to the */
/* diagonal/plane intersection. If this is between -0.5 and 0.5, */
/* the intersection is inside the cube. If so, we continue by */
/* doing a point/triangle intersection. */
/* Do this for all four diagonals. */
d = norm.x * t.v1.x + norm.y * t.v1.y + norm.z * t.v1.z;
/* if one of the diagonals is parallel to the plane, the other will intersect the plane */
if(fabs(denom=(norm.x + norm.y + norm.z))>EPS)
/* skip parallel diagonals to the plane; division by 0 can occur */
{
hitpp.x = hitpp.y = hitpp.z = d / denom;
if (fabs(hitpp.x) <= 0.5)
if (point_triangle_intersection(hitpp,t) == INSIDE) return(INSIDE);
}
if(fabs(denom=(norm.x + norm.y - norm.z))>EPS)
{
hitpn.z = -(hitpn.x = hitpn.y = d / denom);
if (fabs(hitpn.x) <= 0.5)
if (point_triangle_intersection(hitpn,t) == INSIDE) return(INSIDE);
}
if(fabs(denom=(norm.x - norm.y + norm.z))>EPS)
{
hitnp.y = -(hitnp.x = hitnp.z = d / denom);
if (fabs(hitnp.x) <= 0.5)
if (point_triangle_intersection(hitnp,t) == INSIDE) return(INSIDE);
}
if(fabs(denom=(norm.x - norm.y - norm.z))>EPS)
{
hitnn.y = hitnn.z = -(hitnn.x = d / denom);
if (fabs(hitnn.x) <= 0.5)
if (point_triangle_intersection(hitnn,t) == INSIDE) return(INSIDE);
}
/* No edge touched the cube; no cube diagonal touched the triangle. */
/* We're done...there was no intersection. */
return(OUTSIDE);
}
void Az_lsp (
INT16 a[], /* (i) : predictor coefficients */
INT16 lsp[], /* (o) : line spectral pairs */
INT16 old_lsp[] /* (i) : old lsp[] (in case not found 10 roots) */
)
{
INT16 i, j, nf, ip;
INT16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
INT16 x, y, sign, exp;
INT16 *coef;
INT16 f1[M / 2 + 1], f2[M / 2 + 1];
INT32 t0=0;
VPP_EFR_PROFILE_FUNCTION_ENTER(Az_lsp);
/*-------------------------------------------------------------*
* find the sum and diff. pol. F1(z) and F2(z) *
* F1(z) <--- F1(z)/(1+z**-1) & F2(z) <--- F2(z)/(1-z**-1) *
* *
* f1[0] = 1.0; *
* f2[0] = 1.0; *
* *
* for (i = 0; i< NC; i++) *
* { *
* f1[i+1] = a[i+1] + a[M-i] - f1[i] ; *
* f2[i+1] = a[i+1] - a[M-i] + f2[i] ; *
* } *
*-------------------------------------------------------------*/
f1[0] = 1024; /* f1[0] = 1.0 */
f2[0] = 1024; /* f2[0] = 1.0 */
for (i = 0; i < NC; i++)
{
//VPP_MLX16 (t0_hi,t0_lo,a[i + 1], 8192);
//VPP_MLA16 ( t0_hi, t0_lo, a[M - i], 8192);
//t0 = VPP_SCALE64_TO_16( t0_hi, t0_lo);
//x = EXTRACT_H(t0);
t0 = (INT32) a[i + 1] + (INT32)a[M - i];
x = (INT16)(L_SHR_D(t0,2));
/* f1[i+1] = a[i+1] + a[M-i] - f1[i] */
f1[i + 1] = SUB (x, f1[i]);
//VPP_MLX16(t0_hi, t0_lo, a[i + 1], 8192);
//VPP_MLA16(t0_hi, t0_lo, a[M - i], -8192);
//x = EXTRACT_H(VPP_SCALE64_TO_16(t0_hi, t0_lo));
t0 = (INT32) a[i + 1] - (INT32)a[M - i];
x = (INT16)(L_SHR_D(t0,2));
//f2[i + 1] = add (x, f2[i]);
f2[i + 1] = ADD(x, f2[i]);
}
/*-------------------------------------------------------------*
* find the LSPs using the Chebychev pol. evaluation *
*-------------------------------------------------------------*/
nf = 0; /* number of found frequencies */
ip = 0; /* indicator for f1 or f2 */
coef = f1;
xlow = grid[0];
ylow = Chebps (xlow, coef, NC);
j = 0;
/* while ( (nf < M) && (j < grid_points) ) */
//while ((sub (nf, M) < 0) && (sub (j, grid_points) < 0))
while ((SUB (nf, M) < 0) && (SUB (j, grid_points) < 0))
{
j++;
xhigh = xlow;
yhigh = ylow;
xlow = grid[j];
ylow = Chebps (xlow, coef, NC);
//if (L_mult (ylow, yhigh) <= (INT32) 0L)
if (L_MULT(ylow, yhigh) <= (INT32) 0L)
{
/* divide 4 times the interval */
for (i = 0; i < 4; i++)
{
/* xmid = (xlow + xhigh)/2 */
// xmid = add (shr (xlow, 1), shr (xhigh, 1));
xmid = ADD ((SHR_D(xlow, 1)),(SHR_D(xhigh, 1)));
ymid = Chebps (xmid, coef, NC);
//if (L_mult (ylow, ymid) <= (INT32) 0L)
if (L_MULT(ylow, ymid) <= (INT32) 0L)
{
yhigh = ymid;
xhigh = xmid;
}
else
{
ylow = ymid;
xlow = xmid;
}
}
/*-------------------------------------------------------------*
* Linear interpolation *
* xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); *
*-------------------------------------------------------------*/
//x = sub (xhigh, xlow);
x = SUB (xhigh, xlow);
//y = sub (yhigh, ylow);
y = SUB (yhigh, ylow);
if (y == 0)
{
xint = xlow;
}
else
{
sign = y;
//y = abs_s (y);
y = ABS_S(y);
exp = norm_s (y);
//y = shl (y, exp);
y = SHL(y, exp);
y = div_s ((INT16) 16383, y);
//t0 = L_mult (x, y);
t0 = L_MULT(x, y);
//t0 = L_shr (t0, sub (20, exp));
t0 = L_SHR_V(t0, SUB (20, exp));
//y = extract_l (t0); /* y= (xhigh-xlow)/(yhigh-ylow) */
y = EXTRACT_L(t0);
if (sign < 0)
{
//y = negate (y);
y = NEGATE(y);
}
//t0 = L_mult (ylow, y);
t0 = L_MULT(ylow, y);
//t0 = L_shr (t0, 11);
t0 = L_SHR_D(t0, 11);
//xint = sub (xlow, extract_l (t0)); /* xint = xlow - ylow*y */
xint = SUB (xlow, EXTRACT_L(t0));
}
lsp[nf] = xint;
xlow = xint;
nf++;
if (ip == 0)
{
ip = 1;
coef = f2;
}
else
{
ip = 0;
coef = f1;
}
ylow = Chebps (xlow, coef, NC);
}
}
/* Check if M roots found */
//if (sub (nf, M) < 0)
if (SUB (nf, M) < 0)
{
for (i = 0; i < M; i++)
{
lsp[i] = old_lsp[i];
}
}
VPP_EFR_PROFILE_FUNCTION_EXIT(Az_lsp);
return;
}
int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
float U0[3],float U1[3],float U2[3])
{
float E1[3],E2[3];
float N1[3],N2[3],d1,d2;
float du0,du1,du2,dv0,dv1,dv2;
float D[3];
float isect1[2], isect2[2];
float du0du1,du0du2,dv0dv1,dv0dv2;
short index;
float vp0,vp1,vp2;
float up0,up1,up2;
float bb,cc,max;
float a,b,c,x0,x1;
float d,e,f,y0,y1;
float xx,yy,xxyy,tmp;
/* compute plane equation of triangle(V0,V1,V2) */
SUB(E1,V1,V0);
SUB(E2,V2,V0);
CROSS(N1,E1,E2);
d1=-DOT(N1,V0);
/* plane equation 1: N1.X+d1=0 */
/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
du0=DOT(N1,U0)+d1;
du1=DOT(N1,U1)+d1;
du2=DOT(N1,U2)+d1;
/* coplanarity robustness check */
#if USE_EPSILON_TEST==TRUE
if(fabsf(du0)<EPSILON) du0=0.0;
if(fabsf(du1)<EPSILON) du1=0.0;
if(fabsf(du2)<EPSILON) du2=0.0;
#endif
du0du1=du0*du1;
du0du2=du0*du2;
if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
return 0; /* no intersection occurs */
/* compute plane of triangle (U0,U1,U2) */
SUB(E1,U1,U0);
SUB(E2,U2,U0);
CROSS(N2,E1,E2);
d2=-DOT(N2,U0);
/* plane equation 2: N2.X+d2=0 */
/* put V0,V1,V2 into plane equation 2 */
dv0=DOT(N2,V0)+d2;
dv1=DOT(N2,V1)+d2;
dv2=DOT(N2,V2)+d2;
#if USE_EPSILON_TEST==TRUE
if(fabsf(dv0)<EPSILON) dv0=0.0;
if(fabsf(dv1)<EPSILON) dv1=0.0;
if(fabsf(dv2)<EPSILON) dv2=0.0;
#endif
dv0dv1=dv0*dv1;
dv0dv2=dv0*dv2;
if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
return 0; /* no intersection occurs */
/* compute direction of intersection line */
CROSS(D,N1,N2);
/* compute and index to the largest component of D */
max=(float)fabsf(D[0]);
index=0;
bb=(float)fabsf(D[1]);
cc=(float)fabsf(D[2]);
if(bb>max) max=bb,index=1;
if(cc>max) max=cc,index=2;
/* this is the simplified projection onto L*/
vp0=V0[index];
vp1=V1[index];
vp2=V2[index];
up0=U0[index];
up1=U1[index];
up2=U2[index];
/* compute interval for triangle 1 */
NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
/* compute interval for triangle 2 */
NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
xx=x0*x1;
yy=y0*y1;
xxyy=xx*yy;
tmp=a*xxyy;
isect1[0]=tmp+b*x1*yy;
isect1[1]=tmp+c*x0*yy;
tmp=d*xxyy;
isect2[0]=tmp+e*xx*y1;
isect2[1]=tmp+f*xx*y0;
SORT(isect1[0],isect1[1]);
SORT(isect2[0],isect2[1]);
if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
return 1;
}
qboolean tri_tri_intersect(vec3_t V0,vec3_t V1,vec3_t V2,
vec3_t U0,vec3_t U1,vec3_t U2)
{
vec3_t E1,E2;
vec3_t N1,N2;
float d1,d2;
float du0,du1,du2,dv0,dv1,dv2;
vec3_t D;
float isect1[2], isect2[2];
float du0du1,du0du2,dv0dv1,dv0dv2;
short index;
float vp0,vp1,vp2;
float up0,up1,up2;
float b,c,max;
/* compute plane equation of triangle(V0,V1,V2) */
SUB(E1,V1,V0);
SUB(E2,V2,V0);
CROSS(N1,E1,E2);
d1=-DOT(N1,V0);
/* plane equation 1: N1.X+d1=0 */
/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
du0=DOT(N1,U0)+d1;
du1=DOT(N1,U1)+d1;
du2=DOT(N1,U2)+d1;
/* coplanarity robustness check */
#if USE_EPSILON_TEST
if(fabs(du0)<EPSILON) du0=0.0;
if(fabs(du1)<EPSILON) du1=0.0;
if(fabs(du2)<EPSILON) du2=0.0;
#endif
du0du1=du0*du1;
du0du2=du0*du2;
if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
return 0; /* no intersection occurs */
/* compute plane of triangle (U0,U1,U2) */
SUB(E1,U1,U0);
SUB(E2,U2,U0);
CROSS(N2,E1,E2);
d2=-DOT(N2,U0);
/* plane equation 2: N2.X+d2=0 */
/* put V0,V1,V2 into plane equation 2 */
dv0=DOT(N2,V0)+d2;
dv1=DOT(N2,V1)+d2;
dv2=DOT(N2,V2)+d2;
#if USE_EPSILON_TEST
if(fabs(dv0)<EPSILON) dv0=0.0;
if(fabs(dv1)<EPSILON) dv1=0.0;
if(fabs(dv2)<EPSILON) dv2=0.0;
#endif
dv0dv1=dv0*dv1;
dv0dv2=dv0*dv2;
if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
return 0; /* no intersection occurs */
/* compute direction of intersection line */
CROSS(D,N1,N2);
/* compute and index to the largest component of D */
max=fabs(D[0]);
index=0;
b=fabs(D[1]);
c=fabs(D[2]);
if(b>max) max=b,index=1;
if(c>max) max=c,index=2;
/* this is the simplified projection onto L*/
vp0=V0[index];
vp1=V1[index];
vp2=V2[index];
up0=U0[index];
up1=U1[index];
up2=U2[index];
/* compute interval for triangle 1 */
COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
/* compute interval for triangle 2 */
COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
SORT(isect1[0],isect1[1]);
SORT(isect2[0],isect2[1]);
if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return qtrue;
return qfalse;
}
void Jit::WriteDowncount(int offset)
{
const int downcount = js.downcountAmount + offset;
SUB(32, M(¤tMIPS->downcount), downcount > 127 ? Imm32(downcount) : Imm8(downcount));
}
// Sequential version of Karatsuba multiplication, with recursion
// It works well when numbers are about same size
large_int* mult_kar_eqsize(large_int* a, large_int* b) {
int n;
large_int a0,a1,b0,b1;
large_int *a1_plus_a0,*b1_plus_b0, *a1b0_plus_a0b1;
large_int *a1_mult_b1,*a0_mult_b0, *a1pa0_mult_b1pb0;
large_int *temp,*result;
if (debug) printf("Calling mult_kar_eqsize\n");
// For small numbers we use standard multiplication algorithm
// This is the end of the recursion
if (a->size<=KMUX_MINLEVEL || b->size<=KMUX_MINLEVEL) {
if (debug) printf("Calling mult_gmp\n");
// return mult_gmp(a,b);
return mult_s(a,b);
}
// Here we choose first if a or b are smaller. Then out of the smallest one
// which we can call C, we find n that is <= 1/2 of it
if (a->size>b->size) {
n=b->size/2;
if (2*n<b->size) n++;
}
else {
n=a->size/2;
if (2*n<a->size) n++;
}
a0.size=n;
a1.size=a->size-n;
b0.size=n;
b1.size=b->size-n;
// allocate arrays
a0.int_array=malloc(sizeof(unsigned int)*a0.size);
a1.int_array=malloc(sizeof(unsigned int)*a1.size);
b0.int_array=malloc(sizeof(unsigned int)*b0.size);
b1.int_array=malloc(sizeof(unsigned int)*b1.size);
if (a0.int_array==NULL || a1.int_array==NULL || b0.int_array==NULL || b1.int_array==NULL)
die("memory allocation error");
// Copy data from a,b into a0,a1,b0,b1
memcpy(a0.int_array,a->int_array,a0.size*sizeof(unsigned int));
memcpy(a1.int_array,(char*)a->int_array+((size_t)a0.size*sizeof(unsigned int)),a1.size*sizeof(unsigned int));
memcpy(b0.int_array,b->int_array,b0.size*sizeof(unsigned int));
memcpy(b1.int_array,(char*)b->int_array+((size_t)b0.size*sizeof(unsigned int)),b1.size*sizeof(unsigned int));
// Calculate a0+a1, b1+b0 - these can potentially be done in parallel to each other
#pragma omp parallel sections default (shared)
{
a1_plus_a0=ADD(&a1,&a0);
#pragma omp section
b1_plus_b0=ADD(&b1,&b0);
}
// Calculate 3 intermediate products - can all be done in parallel
#pragma omp parallel sections default (shared)
{
if (debug) printf("d0 ");
a1_mult_b1=mult_kar_eqsize(&a1,&b1);
if (debug) { printf("a1*b1 : "); print_hex(a1_mult_b1); }
#pragma omp section
{
a0_mult_b0=mult_kar_eqsize(&a0,&b0);
if (debug) { printf("a0*b0 : "); print_hex(a0_mult_b0); }
}
#pragma omp section
{
a1pa0_mult_b1pb0=mult_kar_eqsize(a1_plus_a0,b1_plus_b0);
if (debug) { printf("(a1+a0)*(b1+b0) : "); print_hex(a1pa0_mult_b1pb0); }
}
}
// From here on things can not really be parallelizeable
if (debug && compare(a1_mult_b1,a1pa0_mult_b1pb0)>0) {
printf("Noticed that a1b1 > (a1+a0)(b1+b0), this should not be\n");
temp=SUB(a1_mult_b1,a1pa0_mult_b1pb0);
temp->sign=-1;
}
else {
if (debug) printf("Calling sub -a1b1 , first_size=%d, second_size=%d\n",a1pa0_mult_b1pb0->size,a1_mult_b1->size);
temp=SUB(a1pa0_mult_b1pb0,a1_mult_b1);
temp->sign=1;
if (debug) print_hex(temp);
}
if (debug && compare(temp,a0_mult_b0)<0) {
if (debug) printf("Noticed that a0b0 > reminder, should not be");
a1b0_plus_a0b1=SUB(a0_mult_b0,temp);
if (temp->sign==-1 && debug) printf("but we're good\n");
else { printf("we have a problem\n"); a1b0_plus_a0b1->sign=-1; }
}
else {
if (debug) printf("calling sub -a0b0: ");
a1b0_plus_a0b1=SUB(temp,a0_mult_b0);
if (temp->sign==-1) printf("we have a problem\n");
if (debug) print_hex(a1b0_plus_a0b1);
}
free(temp->int_array);
free(temp);
// Do some shifting around to prepare final result
shift_left(a1b0_plus_a0b1,n);
shift_left(a1_mult_b1,n*2);
if (debug) {
printf("Shifted a1b0_plus_a0b1: ");
print_hex(a1b0_plus_a0b1);
printf("Shifted a1_mult_b1: ");
print_hex(a1_mult_b1);
}
// Now we're ready to put it all together
temp=ADD(a1_mult_b1,a1b0_plus_a0b1);
result=ADD(temp,a0_mult_b0);
// Free all allocated memory
free(temp->int_array);
free(a1_mult_b1->int_array);
free(a0_mult_b0->int_array);
free(a1pa0_mult_b1pb0->int_array);
free(a1_plus_a0->int_array);
free(b1_plus_b0->int_array);
free(a1b0_plus_a0b1->int_array);
free(a0.int_array);
free(a1.int_array);
free(b0.int_array);
free(b1.int_array);
free(temp);
free(a1_mult_b1);
free(a0_mult_b0);
free(a1pa0_mult_b1pb0);
free(a1_plus_a0);
free(b1_plus_b0);
free(a1b0_plus_a0b1);
// And return
return result;
}
int GGLAssembler::scanline_core(const needs_t& needs, context_t const* c)
{
int64_t duration = ggl_system_time();
mBlendFactorCached = 0;
mBlending = 0;
mMasking = 0;
mAA = GGL_READ_NEEDS(P_AA, needs.p);
mDithering = GGL_READ_NEEDS(P_DITHER, needs.p);
mAlphaTest = GGL_READ_NEEDS(P_ALPHA_TEST, needs.p) + GGL_NEVER;
mDepthTest = GGL_READ_NEEDS(P_DEPTH_TEST, needs.p) + GGL_NEVER;
mFog = GGL_READ_NEEDS(P_FOG, needs.p) != 0;
mSmooth = GGL_READ_NEEDS(SHADE, needs.n) != 0;
mBuilderContext.needs = needs;
mBuilderContext.c = c;
mBuilderContext.Rctx = reserveReg(R0); // context always in R0
mCbFormat = c->formats[ GGL_READ_NEEDS(CB_FORMAT, needs.n) ];
// ------------------------------------------------------------------------
decodeLogicOpNeeds(needs);
decodeTMUNeeds(needs, c);
mBlendSrc = ggl_needs_to_blendfactor(GGL_READ_NEEDS(BLEND_SRC, needs.n));
mBlendDst = ggl_needs_to_blendfactor(GGL_READ_NEEDS(BLEND_DST, needs.n));
mBlendSrcA = ggl_needs_to_blendfactor(GGL_READ_NEEDS(BLEND_SRCA, needs.n));
mBlendDstA = ggl_needs_to_blendfactor(GGL_READ_NEEDS(BLEND_DSTA, needs.n));
if (!mCbFormat.c[GGLFormat::ALPHA].h) {
if ((mBlendSrc == GGL_ONE_MINUS_DST_ALPHA) ||
(mBlendSrc == GGL_DST_ALPHA)) {
mBlendSrc = GGL_ONE;
}
if ((mBlendSrcA == GGL_ONE_MINUS_DST_ALPHA) ||
(mBlendSrcA == GGL_DST_ALPHA)) {
mBlendSrcA = GGL_ONE;
}
if ((mBlendDst == GGL_ONE_MINUS_DST_ALPHA) ||
(mBlendDst == GGL_DST_ALPHA)) {
mBlendDst = GGL_ONE;
}
if ((mBlendDstA == GGL_ONE_MINUS_DST_ALPHA) ||
(mBlendDstA == GGL_DST_ALPHA)) {
mBlendDstA = GGL_ONE;
}
}
// if we need the framebuffer, read it now
const int blending = blending_codes(mBlendSrc, mBlendDst) |
blending_codes(mBlendSrcA, mBlendDstA);
// XXX: handle special cases, destination not modified...
if ((mBlendSrc==GGL_ZERO) && (mBlendSrcA==GGL_ZERO) &&
(mBlendDst==GGL_ONE) && (mBlendDstA==GGL_ONE)) {
// Destination unmodified (beware of logic ops)
} else if ((mBlendSrc==GGL_ZERO) && (mBlendSrcA==GGL_ZERO) &&
(mBlendDst==GGL_ZERO) && (mBlendDstA==GGL_ZERO)) {
// Destination is zero (beware of logic ops)
}
int fbComponents = 0;
const int masking = GGL_READ_NEEDS(MASK_ARGB, needs.n);
for (int i=0 ; i<4 ; i++) {
const int mask = 1<<i;
component_info_t& info = mInfo[i];
int fs = i==GGLFormat::ALPHA ? mBlendSrcA : mBlendSrc;
int fd = i==GGLFormat::ALPHA ? mBlendDstA : mBlendDst;
if (fs==GGL_SRC_ALPHA_SATURATE && i==GGLFormat::ALPHA)
fs = GGL_ONE;
info.masked = !!(masking & mask);
info.inDest = !info.masked && mCbFormat.c[i].h &&
((mLogicOp & LOGIC_OP_SRC) || (!mLogicOp));
if (mCbFormat.components >= GGL_LUMINANCE &&
(i==GGLFormat::GREEN || i==GGLFormat::BLUE)) {
info.inDest = false;
}
info.needed = (i==GGLFormat::ALPHA) &&
(isAlphaSourceNeeded() || mAlphaTest != GGL_ALWAYS);
info.replaced = !!(mTextureMachine.replaced & mask);
info.iterated = (!info.replaced && (info.inDest || info.needed));
info.smooth = mSmooth && info.iterated;
info.fog = mFog && info.inDest && (i != GGLFormat::ALPHA);
info.blend = (fs != int(GGL_ONE)) || (fd > int(GGL_ZERO));
mBlending |= (info.blend ? mask : 0);
mMasking |= (mCbFormat.c[i].h && info.masked) ? mask : 0;
fbComponents |= mCbFormat.c[i].h ? mask : 0;
}
mAllMasked = (mMasking == fbComponents);
if (mAllMasked) {
mDithering = 0;
}
fragment_parts_t parts;
// ------------------------------------------------------------------------
prolog();
// ------------------------------------------------------------------------
build_scanline_prolog(parts, needs);
if (registerFile().status())
return registerFile().status();
// ------------------------------------------------------------------------
label("fragment_loop");
// ------------------------------------------------------------------------
{
Scratch regs(registerFile());
if (mDithering) {
// update the dither index.
MOV(AL, 0, parts.count.reg,
reg_imm(parts.count.reg, ROR, GGL_DITHER_ORDER_SHIFT));
ADD(AL, 0, parts.count.reg, parts.count.reg,
imm( 1 << (32 - GGL_DITHER_ORDER_SHIFT)));
MOV(AL, 0, parts.count.reg,
reg_imm(parts.count.reg, ROR, 32 - GGL_DITHER_ORDER_SHIFT));
}
// XXX: could we do an early alpha-test here in some cases?
// It would probaly be used only with smooth-alpha and no texture
// (or no alpha component in the texture).
// Early z-test
if (mAlphaTest==GGL_ALWAYS) {
build_depth_test(parts, Z_TEST|Z_WRITE);
} else {
// we cannot do the z-write here, because
// it might be killed by the alpha-test later
build_depth_test(parts, Z_TEST);
}
{ // texture coordinates
Scratch scratches(registerFile());
// texel generation
build_textures(parts, regs);
if (registerFile().status())
return registerFile().status();
}
if ((blending & (FACTOR_DST|BLEND_DST)) ||
(mMasking && !mAllMasked) ||
(mLogicOp & LOGIC_OP_DST))
{
// blending / logic_op / masking need the framebuffer
mDstPixel.setTo(regs.obtain(), &mCbFormat);
// load the framebuffer pixel
comment("fetch color-buffer");
load(parts.cbPtr, mDstPixel);
}
if (registerFile().status())
return registerFile().status();
pixel_t pixel;
int directTex = mTextureMachine.directTexture;
if (directTex | parts.packed) {
// note: we can't have both here
// iterated color or direct texture
pixel = directTex ? parts.texel[directTex-1] : parts.iterated;
pixel.flags &= ~CORRUPTIBLE;
} else {
if (mDithering) {
const int ctxtReg = mBuilderContext.Rctx;
const int mask = GGL_DITHER_SIZE-1;
parts.dither = reg_t(regs.obtain());
AND(AL, 0, parts.dither.reg, parts.count.reg, imm(mask));
ADD(AL, 0, parts.dither.reg, parts.dither.reg, ctxtReg);
LDRB(AL, parts.dither.reg, parts.dither.reg,
immed12_pre(GGL_OFFSETOF(ditherMatrix)));
}
// allocate a register for the resulting pixel
pixel.setTo(regs.obtain(), &mCbFormat, FIRST);
build_component(pixel, parts, GGLFormat::ALPHA, regs);
if (mAlphaTest!=GGL_ALWAYS) {
// only handle the z-write part here. We know z-test
// was successful, as well as alpha-test.
build_depth_test(parts, Z_WRITE);
}
build_component(pixel, parts, GGLFormat::RED, regs);
build_component(pixel, parts, GGLFormat::GREEN, regs);
build_component(pixel, parts, GGLFormat::BLUE, regs);
pixel.flags |= CORRUPTIBLE;
}
if (registerFile().status())
return registerFile().status();
if (pixel.reg == -1) {
// be defensive here. if we're here it's probably
// that this whole fragment is a no-op.
pixel = mDstPixel;
}
if (!mAllMasked) {
// logic operation
build_logic_op(pixel, regs);
// masking
build_masking(pixel, regs);
comment("store");
store(parts.cbPtr, pixel, WRITE_BACK);
}
}
if (registerFile().status())
return registerFile().status();
// update the iterated color...
if (parts.reload != 3) {
build_smooth_shade(parts);
}
// update iterated z
build_iterate_z(parts);
// update iterated fog
build_iterate_f(parts);
SUB(AL, S, parts.count.reg, parts.count.reg, imm(1<<16));
B(PL, "fragment_loop");
label("epilog");
epilog(registerFile().touched());
if ((mAlphaTest!=GGL_ALWAYS) || (mDepthTest!=GGL_ALWAYS)) {
if (mDepthTest!=GGL_ALWAYS) {
label("discard_before_textures");
build_iterate_texture_coordinates(parts);
}
label("discard_after_textures");
build_smooth_shade(parts);
build_iterate_z(parts);
build_iterate_f(parts);
if (!mAllMasked) {
ADD(AL, 0, parts.cbPtr.reg, parts.cbPtr.reg, imm(parts.cbPtr.size>>3));
}
SUB(AL, S, parts.count.reg, parts.count.reg, imm(1<<16));
B(PL, "fragment_loop");
epilog(registerFile().touched());
}
void updateE(N3 N, P3F3 E, P3F3 H, P3F3 CE) {
int i,j,k;
v4sf e, ce, h1, h2, h3, h4;
for (i=0;i<N.x-1;i++){
for (j=0;j<N.y-1;j++){
for (k=0;k<N.z;k+=4){
e = LOAD(&E.x[i][j][k]);
ce = LOAD(&CE.x[i][j][k]);
h1 = LOAD(&H.z[i][j+1][k]);
h2 = LOAD(&H.z[i][j][k]);
h3 = LOAD(&H.y[i][j][k+1]);
h4 = LOAD(&H.y[i][j][k]);
STORE(&E.x[i][j][k], ADD(e, MUL(ce, SUB( SUB(h1,h2), SUB(h3,h4)))));
}
}
}
for (i=0;i<N.x-1;i++){
for (j=0;j<N.y-1;j++){
for (k=0;k<N.z;k+=4){
e = LOAD(&E.y[i][j][k]);
ce = LOAD(&CE.y[i][j][k]);
h1 = LOAD(&H.x[i][j][k+1]);
h2 = LOAD(&H.x[i][j][k]);
h3 = LOAD(&H.z[i+1][j][k]);
h4 = LOAD(&H.z[i][j][k]);
STORE(&E.y[i][j][k], ADD(e, MUL(ce, SUB( SUB(h1,h2), SUB(h3,h4)))));
}
}
}
for (i=0;i<N.x-1;i++){
for (j=0;j<N.y-1;j++){
for (k=0;k<N.z;k+=4){
e = LOAD(&E.z[i][j][k]);
ce = LOAD(&CE.z[i][j][k]);
h1 = LOAD(&H.y[i+1][j][k]);
h2 = LOAD(&H.y[i][j][k]);
h3 = LOAD(&H.x[i][j+1][k]);
h4 = LOAD(&H.x[i][j][k]);
STORE(&E.z[i][j][k], ADD(e, MUL(ce, SUB( SUB(h1,h2), SUB(h3,h4)))));
}
}
}
i=N.x-1;
for (j=0;j<N.y-1;j++){
for (k=0;k<N.z;k+=4){
e = LOAD(&E.x[i][j][k]);
ce = LOAD(&CE.x[i][j][k]);
h1 = LOAD(&H.z[i][j+1][k]);
h2 = LOAD(&H.z[i][j][k]);
h3 = LOAD(&H.y[i][j][k+1]);
h4 = LOAD(&H.y[i][j][k]);
STORE(&E.x[i][j][k], ADD(e, MUL(ce, SUB( SUB(h1,h2), SUB(h3,h4)))));
}
}
j=N.y-1;
for (i=0;i<N.x-1;i++){
for (k=0;k<N.z;k+=4){
e = LOAD(&E.y[i][j][k]);
ce = LOAD(&CE.y[i][j][k]);
h1 = LOAD(&H.x[i][j][k+1]);
h2 = LOAD(&H.x[i][j][k]);
h3 = LOAD(&H.z[i+1][j][k]);
h4 = LOAD(&H.z[i][j][k]);
STORE(&E.y[i][j][k], ADD(e, MUL(ce, SUB( SUB(h1,h2), SUB(h3,h4)))));
}
}
}
/*
called by quicksort() - does the actual sorting
arg - to pass the structure of the sorting arguments
//*/
static void* _quicksort(void *arg)
{
// set the sorting arguments locally
sort_args_t *sargs = (sort_args_t *) arg;
// base pointer - current pointer to the element, set to base
register void *base_pointer = sargs->sortargs_base;
// number of elements (size)
int number_of_elements = sargs->sortargs_number_of_elements;
// the offset size of the elements (each?)
int width = sargs->sortargs_width;
// set the comparison function (pointer) to the function set in the arguments
int (*compar_func)(const void *, const void *) = sargs->sortargs_compar_func;
// counding indexes
register int idx;
register int jdx;
// the result of the comparison
int result;
int thread_count = 0;
// tmp - temporary storage for an element at swapping
void *tmp;
// tmp_base - temporary base pointer, pointer to an element
void *tmp_base[3];
void *pivot = 0;
sort_args_t sort_args[2];
pthread_t tid;
// find the pivot point
switch(number_of_elements){
case 0:
case 1:
return 0;
case 2:
if((*compar_func)(SUB(base_pointer, 0), SUB(base_pointer, 1)) > 0){
SWAP(base_pointer, 0, 1, width);
}
return 0;
case 3:
// three sort
if((*compar_func)(SUB(base_pointer, 0), SUB(base_pointer, 1)) > 0){
SWAP(base_pointer, 0, 1, width);
}
// the first two are now ordered, now order the second two
if((*compar_func)(SUB(base_pointer, 2), SUB(base_pointer, 1)) < 0){
SWAP(base_pointer, 2, 1, width);
}
// should the second be moved to the first?
if((*compar_func)(SUB(base_pointer, 1), SUB(base_pointer, 0)) < 0){
SWAP(base_pointer, 1, 0, width);
}
return 0;
default:
if(number_of_elements > 3){
tmp_base[0] = SUB(base_pointer, 0);
tmp_base[1] = SUB(base_pointer, number_of_elements / 2);
tmp_base[2] = SUB(base_pointer, number_of_elements - 1);
// three sort
if((*compar_func)(tmp_base[0], tmp_base[1]) > 0){
tmp = tmp_base[0];
tmp_base[0] = tmp_base[1];
tmp_base[1] = tmp;
}
// the first two are now ordered, now order the second two
if((*compar_func)(tmp_base[2], tmp_base[1]) < 0){
tmp = tmp_base[1];
tmp_base[1] = tmp_base[2];
tmp_base[2] = tmp;
}
// should the second be moved to the first?
if((*compar_func)(tmp_base[1], tmp_base[0]) < 0){
tmp = tmp_base[0];
tmp_base[0] = tmp_base[1];
tmp_base[1] = tmp;
}
if((*compar_func)(tmp_base[0], tmp_base[2]) != 0){
if((*compar_func)(tmp_base[0], tmp_base[1]) < 0){
pivot = tmp_base[1];
}else{
pivot = tmp_base[2];
}
}
}
break;
}
if(pivot == 0){
for(idx = 1; idx < number_of_elements; ++idx){
if(0 != (result = ((*compar_func) (SUB(base_pointer, 0), SUB(base_pointer, idx))))){
pivot = (result > 0) ? SUB(base_pointer, 0) : SUB(base_pointer, idx);
break;
}
}
}
if (pivot == 0)
return 0;
/*
sort
idx - starting from 0
jdx - starting from the last elements index
base_pointer - pointer to the current element
//*/
idx = 0;
jdx = number_of_elements - 1;
while(idx <= jdx){
while((*compar_func)(SUB(base_pointer, idx), pivot) < 0)
++idx;
while((*compar_func)(SUB(base_pointer, jdx), pivot) >= 0)
--jdx;
if(idx < jdx){
SWAP(base_pointer, idx, jdx, width);
++idx;
--jdx;
}
}
// sort the sides judiciously
switch(idx){
case 0:
case 1:
break;
case 2:
if((*compar_func)(SUB(base_pointer, 0), SUB(base_pointer, 1)) > 0) {
SWAP(base_pointer, 0, 1, width);
}
break;
case 3:
// three sort
if((*compar_func)(SUB(base_pointer, 0), SUB(base_pointer, 1)) > 0) {
SWAP(base_pointer, 0, 1, width);
}
// the first two are now ordered, now order the second two
if ((*compar_func)(SUB(base_pointer, 2), SUB(base_pointer, 1)) < 0) {
SWAP(base_pointer, 2, 1, width);
}
// should the second be moved to the first?
if ((*compar_func)(SUB(base_pointer, 1), SUB(base_pointer, 0)) < 0) {
SWAP(base_pointer, 1, 0, width);
}
break;
default:
// reinit the sorting arguments
sort_args[0].sortargs_base = base_pointer;
sort_args[0].sortargs_number_of_elements = idx;
sort_args[0].sortargs_width = width;
sort_args[0].sortargs_compar_func = compar_func;
// call the _quicksort method recursively, creating threads if possible
if((threads_avail > 0) && (idx > SLICE_THRESH)) {
threads_avail--;
pthread_create(&tid, NULL, _quicksort, &sort_args[0]);
thread_count = 1;
} else
_quicksort(&sort_args[0]);
break;
}
// shrink
jdx = number_of_elements - idx;
switch(jdx){
case 1:
break;
case 2:
if((*compar_func)(SUB(base_pointer, idx), SUB(base_pointer, idx + 1)) > 0){
SWAP(base_pointer, idx, idx + 1, width);
}
break;
case 3:
// three sort
if((*compar_func)(SUB(base_pointer, idx), SUB(base_pointer, idx + 1)) > 0){
SWAP(base_pointer, idx, idx + 1, width);
}
// the first two are now ordered, now order the second two
if((*compar_func)(SUB(base_pointer, idx + 2), SUB(base_pointer, idx + 1)) < 0){
SWAP(base_pointer, idx + 2, idx + 1, width);
}
// should the second be moved to the first?
if((*compar_func)(SUB(base_pointer, idx + 1), SUB(base_pointer, idx)) < 0) {
SWAP(base_pointer, idx + 1, idx, width);
}
break;
default:
// reinit the sorting arguments
sort_args[1].sortargs_base = SUB(base_pointer, idx);
sort_args[1].sortargs_number_of_elements = jdx;
sort_args[1].sortargs_width = width;
sort_args[1].sortargs_compar_func = compar_func;
// call _quicksort recursively, creating threads if possible
if((thread_count == 0) && (threads_avail > 0) && (idx > SLICE_THRESH)) {
threads_avail--;
pthread_create(&tid, NULL, _quicksort, &sort_args[1]);
thread_count = 1;
}else{
_quicksort(&sort_args[1]);
}
break;
}
if(thread_count){
pthread_join(tid, NULL);
threads_avail++;
}
return 0;
}