double
TE::angle_diff(double a, double b) {
double d1, d2;
a = NORMALIZE(a);
b = NORMALIZE(b);
d1 = a - b;
d2 = 2 * M_PI - fabs(d1);
if (d1 > 0)
d2 *= -1.0;
if (fabs(d1) < fabs(d2))
return (d1);
else
return (d2);
}
static double ANGLEDIFF (double a, double b)
{
double d1, d2;
a = NORMALIZE(a);
b = NORMALIZE(b);
d1 = a-b;
d2 = 2*M_PI - fabs(d1);
if(d1 > 0)
d2 *= -1.0;
if(fabs(d1) < fabs(d2))
return(d1);
else
return(d2);
}
void rtgui_dc_trans_get_new_wh(struct rtgui_dc_trans *dct,
int *new_wp,
int *new_hp)
{
struct rtgui_rect rect;
struct rtgui_point topleft, topright, bottomright;
RT_ASSERT(dct);
if (!new_wp && !new_hp)
return;
rtgui_dc_get_rect(dct->owner, &rect);
/* We ignore the movement components in the matrix. */
/* Transform result of (0, h). */
rtgui_matrix_mul_point_nomove(&topleft, 0, rect.y2,
&dct->m);
/* Transform result of (w, h). */
rtgui_matrix_mul_point_nomove(&topright, rect.x2, rect.y2,
&dct->m);
/* Transform result of (w, 0). */
rtgui_matrix_mul_point_nomove(&bottomright,
rect.x2, 0, &dct->m);
/* Transform result of (0, 0) is always (0, 0). */
#define NORMALIZE(x) do { if (x < 0) x = 0; } while (0)
if (new_wp)
{
int neww;
/* Ignore the nagtive parts. */
NORMALIZE(topright.x);
NORMALIZE(topleft.x);
NORMALIZE(bottomright.x);
neww = _UI_MAX(topright.x, _UI_ABS(topleft.x - bottomright.x))
+ dct->m.m[4] + 1;
NORMALIZE(neww);
*new_wp = neww;
}
if (new_hp)
{
int newh;
NORMALIZE(topright.y);
NORMALIZE(topleft.y);
NORMALIZE(bottomright.y);
newh = _UI_MAX(topright.y, _UI_ABS(topleft.y - bottomright.y))
+ dct->m.m[5] + 1;
NORMALIZE(newh);
*new_hp = newh;
}
#undef NORMALIZE
}
int mollweide(mapx_class *current, double lat, double lon,
double *x, double *y)
{
double dlon;
double phi, lam, theta, delta;
double sin_theta, cos_theta, psi, epsilon=1e-6;
int it, maxit=10;
dlon = lon - current->lon0;
NORMALIZE(dlon);
phi = RADIANS (lat);
lam = RADIANS (dlon);
delta = 0.0;
theta = phi;
sin_theta = sin(theta);
cos_theta = cos(theta);
if (fabs(cos_theta) > epsilon)
{ psi = PI*sin(phi);
it = 0;
repeat
{ delta = -(theta + sin_theta - psi) / (1 + cos_theta);
theta += delta;
sin_theta = sin(theta);
cos_theta = cos(theta);
if (++it >= maxit) break;
} until (fabs(delta) <= epsilon);
theta /= 2.0;
sin_theta = sin(theta);
cos_theta = cos(theta);
}
int check_line(t_point *res, t_point *pos, t_point *dir, int max)
{
int index;
int tmp_x;
int tmp_y;
tmp_x = pos->x;
tmp_y = pos->y;
while (tmp_x != max && tmp_y != max)
{
if (IS_IN_MAP(PLAYER_X) && IS_IN_MAP(PLAYER_Y))
{
index = NORMALIZE(PLAYER_X, PLAYER_Y);
if (IS_ENEMY(g_shm.data[index]))
{
res->x = tmp_x;
res->y = tmp_y;
return (1);
}
}
tmp_x += dir->x;
tmp_y += dir->y;
}
return (0);
}
// -----------------------------------------------------------
// Engine::Raytrace
// Naive ray tracing: Intersects the ray with every primitive
// in the scene to determine the closest intersection
// -----------------------------------------------------------
Primitive* Engine::Raytrace( Ray& a_Ray, Color& a_Acc, int a_Depth, float a_RIndex, float& a_Dist )
{
if (a_Depth > TRACEDEPTH) return 0;
// trace primary ray
a_Dist = 1000000.0f;
vector3 pi;
Primitive* prim = 0;
int result;
// find the nearest intersection
for ( int s = 0; s < m_Scene->GetNrPrimitives(); s++ )
{
Primitive* pr = m_Scene->GetPrimitive( s );
int res;
if (res = pr->Intersect( a_Ray, a_Dist ))
{
prim = pr;
result = res; // 0 = miss, 1 = hit, -1 = hit from inside primitive
}
}
// no hit, terminate ray
if (!prim) return 0;
// handle intersection
if (prim->IsLight())
{
// we hit a light, stop tracing
a_Acc = Color( 1, 1, 1 );
}
else
{
// determine color at point of intersection
pi = a_Ray.GetOrigin() + a_Ray.GetDirection() * a_Dist;
// trace lights
for ( int l = 0; l < m_Scene->GetNrPrimitives(); l++ )
{
Primitive* p = m_Scene->GetPrimitive( l );
if (p->IsLight())
{
Primitive* light = p;
// calculate diffuse shading
vector3 L = ((Sphere*)light)->GetCentre() - pi;
NORMALIZE( L );
vector3 N = prim->GetNormal( pi );
if (prim->GetMaterial()->GetDiffuse() > 0)
{
float dot = DOT( N, L );
if (dot > 0)
{
float diff = dot * prim->GetMaterial()->GetDiffuse();
// add diffuse component to ray color
a_Acc += diff * prim->GetMaterial()->GetColor() * light->GetMaterial()->GetColor();
}
}
}
}
}
// return pointer to primitive hit by primary ray
return prim;
}
float vParticleImpulseGen::GetImpulse( vParticle *parent )
{
Assert( parent != NULL );
float val = 0;
switch ( mode )
{
case PARTICLEIMPULSEGENERATOR_FRAMETIME:
val = CFrameTimeHelper::GetFrameTime() * impulse_multiplier;
break;
case PARTICLEIMPULSEGENERATOR_LIFETIME_LINEAR:
val = NORMALIZE( ( CFrameTimeHelper::GetCurrentTime() - parent->GetCreationTime() ) / parent->GetLifeDuration() ) * impulse_multiplier;
break;
case PARTICLEIMPULSEGENERATOR_LIFETIME_SINE:
val = NORMALIZE( ( CFrameTimeHelper::GetCurrentTime() - parent->GetCreationTime() ) / parent->GetLifeDuration() );
val = NORMALIZE( abs( sin( M_PI_F * val * impulse_multiplier ) ) );
break;
case PARTICLEIMPULSEGENERATOR_VELOCITY_LINEAR:
val = parent->vecVelocity.Length() * impulse_multiplier;
break;
case PARTICLEIMPULSEGENERATOR_ANGULAR_VELOCITY_LINEAR:
val = abs( parent->flAngularVelocity ) * impulse_multiplier;
break;
case PARTICLEIMPULSEGENERATOR_CURTIME_SINE:
val = NORMALIZE( abs( sin( M_PI_F * CFrameTimeHelper::GetCurrentTime() * impulse_multiplier ) ) );
break;
case PARTICLEIMPULSEGENERATOR_CURTIME_SINE_SIGNED:
val = ( ( sin( M_PI_F * CFrameTimeHelper::GetCurrentTime() * impulse_multiplier ) ) );
break;
default:
Assert(0);
break;
}
Assert( IsFinite(val) );
float sign = Sign( val );
val = sign * Bias( abs(val), impulse_bias );
val = RemapVal( val, 0.0f, 1.0f, reference_min, reference_max );
return val;
}
/*------------------------------------------------------------------------
* move_pu - move pen up function.
*
* input: lat, lon - beginning of a new segment
*
* result: previous segment is written, a new segment index entry is started,
* and the map is recentered.
*
* return: FALSE on success
* TRUE if fatal error occurs (unabel to allocate enough memory)
*------------------------------------------------------------------------*/
int move_pu(double lat, double lon)
{
double nlon;
/*
* write current segment
*/
if (dest->npoints > 0) write_segment_data(dest->seg_count);
/*
* make sure index is big enough
*/
if (dest->seg_count >= max_index_entries)
{ max_index_entries += 1000;
dest->index =
(cdb_index_entry *) realloc(dest->index,
sizeof(cdb_index_entry) * max_index_entries);
if(NULL == dest->index)
{
fprintf(stderr,"move_pu: Unable to allocate %d index entries\n",
max_index_entries);
return -1;
}
if (verbose) fprintf(stderr,">allocating %d index entries.\n",
max_index_entries);
}
/*
* start a new segment
*/
dest->index[dest->seg_count].ID = dest->seg_count;
dest->index[dest->seg_count].ilat0 = nint(lat/CDB_LAT_SCALE);
nlon = lon;
NORMALIZE(nlon);
dest->index[dest->seg_count].ilon0 = nint(nlon/CDB_LON_SCALE);
dest->index[dest->seg_count].ilat_max = dest->index[dest->seg_count].ilat0;
dest->index[dest->seg_count].ilon_max = dest->index[dest->seg_count].ilon0;
dest->index[dest->seg_count].ilat_min = dest->index[dest->seg_count].ilat0;
dest->index[dest->seg_count].ilon_min = dest->index[dest->seg_count].ilon0;
/*
* recenter map
*/
map->center_lat = lat;
map->center_lon = nlon;
map->lat0 = lat;
map->lon0 = nlon;
reinit_mapx(map);
dest->npoints = 0;
if (very_very_verbose)fprintf(stderr,">>> recentered map to %lf %lf.\n",
map->lat0, map->lon0);
return 0;
}
static void setup_normal (REAL (*nodes) [3], FACE *fac)
{
int n0, n1, n2;
REAL *normal;
n0 = fac->nodes [0];
n1 = fac->nodes [1];
n2 = fac->nodes [2];
normal = fac->normal;
NORMAL (nodes [n0], nodes [n1], nodes [n2], normal);
NORMALIZE (normal);
}
/*
* Deduplicate function: it writes the chunk in the hash table
* using the hash parameter as a key.
*
* It returns: 1 if it successes
* 0 if not
*
* If the chunk exists, the content is put in the retrieved_chunk buffer
*
* */
int put_block(hashtable *as, unsigned char *buffer, uint32_t hash){
int i;
// Insert buffer in the hash table using the normalized "hash" variable as key
// We normalize the key according o the table size
i = as_escribir(as, NORMALIZE(hash), buffer);
if(i){
// printf("Ok, everything is gonna be alright! \n");
return 1;
}else{
printf("Error inserting in the table\n");
return 0;
}
}
static int
constraintSatIso(pullTask *task, pullPoint *point,
double stepMax, unsigned int iterMax,
/* output */
int *constrFailP) {
static const char me[]="constraintSatIso";
double
step, /* current step size */
val, aval, /* last and current function values */
hack, /* how to control re-tries in the context of a single
for-loop, instead of a nested do-while loop */
grad[4], dir[3], len, state[1 + 1 + 3 + 3];
unsigned int iter = 0; /* 0: initial probe, 1..iterMax: probes in loop */
PROBE(val, aval, grad);
SAVE(state, aval, val, grad, point->pos);
hack = 1;
for (iter=1; iter<=iterMax; iter++) {
/* consider? http://en.wikipedia.org/wiki/Halley%27s_method */
NORMALIZE(dir, grad, len);
if (!len) {
/* no gradient; back off */
hack *= task->pctx->sysParm.backStepScale;
RESTORE(aval, val, grad, point->pos, state);
continue;
}
step = -val/len; /* the newton-raphson step */
step = step > 0 ? AIR_MIN(stepMax, step) : AIR_MAX(-stepMax, step);
ELL_3V_SCALE_INCR(point->pos, hack*step, dir);
_pullPointHistAdd(point, pullCondConstraintSatA);
PROBE(val, aval, grad);
if (aval <= state[0]) { /* we're no further from the root */
if (AIR_ABS(step) < stepMax*task->pctx->sysParm.constraintStepMin) {
/* we have converged! */
break;
}
SAVE(state, aval, val, grad, point->pos);
hack = 1;
} else { /* oops, try again, don't update dir or len, reset val */
hack *= task->pctx->sysParm.backStepScale;
RESTORE(aval, val, grad, point->pos, state);
}
}
if (iter > iterMax) {
*constrFailP = pullConstraintFailIterMaxed;
} else {
*constrFailP = AIR_FALSE;
}
return 0;
}
inline void WaypointFollower::turnVelocityInGoal(VELOCITY_COMMAND & cmd) {
double final_turning_angle = 0.0;
final_turning_angle = NORMALIZE(this->final_gaz - this->az);
ROS_DEBUG_NAMED("velo","Normalize(%f - %f) = %f", this->final_gaz, this->az, final_turning_angle);
ROS_DEBUG_NAMED("velo","Reaching final angle from %f to %f = %f", this->az, this->final_gaz, final_turning_angle);
cmd.vel.px = 0;
if (fabs(final_turning_angle) < this->motionParams.az_success_distance) {
cmd.vel.pa = 0;
ROS_INFO("Goal reached at angle %f < %f", fabs(final_turning_angle) , this->motionParams.az_success_distance);
this->goal_ready = false;
this->robot_movement_allowed = false; // wait for new goal
} else {
cmd.vel.pa = ABSMAX( ( final_turning_angle * this->motionParams.pid_rot_kp ), this->motionParams.max_rot_vel );
}
}
/*
* The intention here is to remove the large spacial frequencies from the data.
* The data along each scan is fit with a low order polynomial, which is subtracted out.
*/
void flatten_field(FluxWappData * wappdata, float nsigma, int order)
{
int r, i, j;
int num_records;
double RA;
double min, max;
double dI[MAX_SCAN_SIZE];
double dRA[MAX_SCAN_SIZE];
double chisq[4];
double cI[20];
for (r=0; r<wappdata->numDays; r++)
{
ScanDayData * daydata = &wappdata->scanDayData[r];
for (i=0; i<daydata->numScans; i++)
{
ScanData * scan = &daydata->scans[i];
num_records = scan->num_records;
//printf("day: %i scan: %i\n", r, i);
//check for array overflow
if (num_records > MAX_SCAN_SIZE) {
printf("ERROR: MAX_SCAN_SIZE (%i) is smaller than num_records (%i)\n", MAX_SCAN_SIZE, num_records);
return;
}
//create the data arrays for fitting
for (j=0; j<num_records; j++) {
dRA[j] = scan->records[j].RA;
dI[j] = scan->records[j].stokes.I;
}
//fit the data on the scan with a polynomial
jsd_minmax(dRA, num_records, &min, &max);
jsd_normalize(dRA, num_records, min, max);
jsd_poly_fit(dRA, dI, num_records, nsigma, cI, order, &chisq[0]);
//subtract out the polynomial evaluation from the data
for (j=0; j<num_records; j++) {
RA = NORMALIZE(scan->records[j].RA, min, max);
scan->records[j].stokes.I -= jsd_poly_eval(RA, cI, order);
}
}
}
}
/*
* normalize process parameters (Offset-->Pointer)
*
* @implemented
*/
PRTL_USER_PROCESS_PARAMETERS NTAPI
RtlNormalizeProcessParams(PRTL_USER_PROCESS_PARAMETERS Params)
{
if (Params && !(Params->Flags & RTL_USER_PROCESS_PARAMETERS_NORMALIZED))
{
NORMALIZE(Params->CurrentDirectory.DosPath.Buffer, Params);
NORMALIZE(Params->DllPath.Buffer, Params);
NORMALIZE(Params->ImagePathName.Buffer, Params);
NORMALIZE(Params->CommandLine.Buffer, Params);
NORMALIZE(Params->WindowTitle.Buffer, Params);
NORMALIZE(Params->DesktopInfo.Buffer, Params);
NORMALIZE(Params->ShellInfo.Buffer, Params);
NORMALIZE(Params->RuntimeData.Buffer, Params);
Params->Flags |= RTL_USER_PROCESS_PARAMETERS_NORMALIZED;
}
return Params;
}
int check_collision(hashtable *as, unsigned char *block_ptr, uint32_t hash){
int i;
char ptr[CHUNK];
i =as_leer(as, NORMALIZE(hash), &ptr);
if (i){
i = memcmp (ptr, block_ptr, CHUNK);
if(i != 0){
// printf("\n%d\n",i);
// printf("\nCollision!!!!\n");
return 0;
}
// fwrite(ptr,1, CHUNK, stdout);
}
return 1;
}
int inverse_cylindrical_equidistant (mapx_class *current,
double x, double y,
double *lat, double *lon)
{
double phi, lam;
x -= current->false_easting;
y -= current->false_northing;
phi = y/current->Rg;
lam = x/(current->Rg*current->cos_phi1);
*lat = DEGREES(phi);
*lon = DEGREES(lam) + current->lon0;
NORMALIZE(*lon);
return 0;
}
// -----------------------------------------------------------
// Engine::Render
// Fires rays in the scene one scanline at a time, from left
// to right
// -----------------------------------------------------------
bool Engine::Render()
{
// render scene
vector3 o( 0, 0, -5 );
// initialize timer
int msecs = GetTickCount();
// reset last found primitive pointer
Primitive* lastprim = 0;
// render remaining lines
for ( int y = m_CurrLine; y < (m_Height - 20); y++ )
{
m_SX = m_WX1;
// render pixels for current line
for ( int x = 0; x < m_Width; x++ )
{
// fire primary ray
Color acc( 0, 0, 0 );
vector3 dir = vector3( m_SX, m_SY, 0 ) - o;
NORMALIZE( dir );
Ray r( o, dir );
float dist;
Primitive* prim = Raytrace( r, acc, 1, 1.0f, dist );
int red = (int)(acc.r * 256);
int green = (int)(acc.g * 256);
int blue = (int)(acc.b * 256);
if (red > 255) red = 255;
if (green > 255) green = 255;
if (blue > 255) blue = 255;
m_Dest[m_PPos++] = (red << 16) + (green << 8) + blue;
m_SX += m_DX;
}
m_SY += m_DY;
// see if we've been working to long already
if ((GetTickCount() - msecs) > 100)
{
// return control to windows so the screen gets updated
m_CurrLine = y + 1;
return false;
}
}
// all done
return true;
}
/*
* Regenerate function: it checks the presence of a chunk in the
* hash table pointed by *as and returns the content.
*
* It returns: 1 if the chunk exists
* 0 if not
*
* If the chunk exists, the content is put in the retrieved_chunk buffer
*
* */
int get_block(uint32_t hash, hashtable *as, unsigned char *retrieved_chunk){
// int i;
// printf("\nHash:%u\n",hash);
// printf("Hashed retrieved from message:%u\n", hash);
// printf("Normalized value of hash:%u\n", NORMALIZE(hash));
// Look for the table entry
return as_leer(as, NORMALIZE(hash), retrieved_chunk);
// if(i){ // Find!
// printf("Deduplicated value: \n");
// Write to the stdout
// fwrite(retrieved_chunk, 1, CHUNK, stdout);
// Write the deduplicated chunk to the output file
// fwrite(retrieved_chunk,1,CHUNK,output_file);
// return 1;
// printf("%s\n",buffer);
// }else{ // Nothing!
// printf("Not found \n");
// return 0;
// }
}
int cylindrical_equidistant (mapx_class *current,
double lat, double lon, double *x, double *y)
{
double dlon;
double phi, lam;
dlon = lon - current->lon0;
NORMALIZE(dlon);
phi = RADIANS (lat);
lam = RADIANS (dlon);
*x = current->Rg * lam * current->cos_phi1;
*y = current->Rg * phi;
*x += current->false_easting;
*y += current->false_northing;
return 0;
}
/* Calculate the position of the sun at a given time. pages 89-91 */
void
sun_position (time_t unix_time, gdouble *lat, gdouble *lon)
{
gdouble jd, D, N, M, E, x, v, lambda;
gdouble ra, dec;
jd = unix_time_to_julian_date (unix_time);
/* Calculate number of days since the epoch */
D = jd - EPOCH;
N = D*360/365.242191;
/* normalize to 0 - 360 degrees */
NORMALIZE (N);
/* Step 4: */
M = N + EPSILON_G - MU_G;
NORMALIZE (M);
/* Step 5: convert to radians */
M = DEG_TO_RADS (M);
/* Step 6: */
E = solve_keplers_equation (ECCENTRICITY, M);
/* Step 7: */
x = sqrt ((1 + ECCENTRICITY)/(1 - ECCENTRICITY)) * tan (E/2);
/* Step 8, 9 */
v = 2 * RADS_TO_DEG (atan (x));
NORMALIZE (v);
/* Step 10 */
lambda = v + MU_G;
NORMALIZE (lambda);
/* convert the ecliptic longitude to right ascension and declination */
ecliptic_to_equatorial (DEG_TO_RADS (lambda), 0.0, &ra, &dec);
ra = ra - (G_PI/12) * greenwich_sidereal_time (unix_time);
ra = RADS_TO_DEG (ra);
dec = RADS_TO_DEG (dec);
NORMALIZE (ra);
NORMALIZE (dec);
*lat = dec;
*lon = ra;
}
/* rotation of a ellipsoid */
void ELLIP_Rotate (ELLIP *eli, double *point, double *vector, double angle)
{
double R [9], omega [3];
double *ref [4] = {eli->ref_center,
eli->ref_point [0],
eli->ref_point [1],
eli->ref_point [2]};
angle *= ALG_PI / 180.0;
COPY (vector, omega);
NORMALIZE (omega);
SCALE (omega, angle);
EXPMAP (omega, R);
for (int i = 0; i < 4; i ++)
{
SUB (ref [i], point, omega);
NVADDMUL (point, R, omega, ref [i]);
}
ELLIP_Update (eli, NULL, NULL, NULL);
}
/* rotation of a sphere */
void SPHERE_Rotate (SPHERE *sph, double *point, double *vector, double angle)
{
double R [9], omega [3];
double (*ref_pnt) [3] = sph->ref_point,
(*cur_pnt) [3] = sph->cur_point;
angle *= ALG_PI / 180.0;
COPY (vector, omega);
NORMALIZE (omega);
SCALE (omega, angle);
EXPMAP (omega, R);
SUB (sph->cur_center, point, omega);
NVADDMUL (point, R, omega, sph->cur_center);
COPY (sph->cur_center, sph->ref_center);
for (int i = 0; i < 3; i ++)
{
SUB (cur_pnt [i], point, omega);
NVADDMUL (point, R, omega, cur_pnt [i]);
COPY (cur_pnt [i], ref_pnt [i]);
}
}
int integer_square(integer_t target, integer_t source) {
bool aliased;
integer_t result;
if(integer_is_zero(source)) { return integer_set_unsigned_value(target, 0); }
aliased = (result == source);
result = aliased ? integer_create(0) : target;
VERIFY(GROW(result, 2 * LENGTH(source)));
SET_SIGNED_LENGTH(result, 2 * LENGTH(source));
VERIFY(digits_square(DIGITS(result), DIGITS(source), LENGTH(source)));
NORMALIZE(result);
// TODO: call a karatsuba routine here...
if(aliased) {
EXCHANGE(target, result);
integer_destroy(result);
}
return 0;
}
int inverse_albers_conic_equal_area(mapx_class *current, double x, double y,
double *lat, double *lon)
{
double phi, lam, rho, rmy, theta, chi;
x -= current->false_easting;
y -= current->false_northing;
rmy = current->rho0 - y;
rho = sqrt(x*x + rmy*rmy);
theta = current->n >= 0 ?
atan2( x, rmy) :
atan2(-x, -rmy);
chi = rho*current->n/current->Rg;
phi = asin((current->C - chi*chi)/(2*current->n));
lam = theta/current->n;
*lat = DEGREES(phi);
*lon = DEGREES(lam) + current->lon0;
NORMALIZE(*lon);
return 0;
}
/* 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;
}
/*
* Check functions: it checks the presence of a chunk in the
* hash table pointed by *as.
*
* It returns: 1 if the chunk exists
* 0 if not
* */
int check(hashtable *as, uint32_t hash){
char tmp[CHUNK];
return as_leer(as, NORMALIZE(hash), &tmp);
}
float cs_svd_1( int nn , int mm , float *xx , float *evec )
{
register int ii,jj,n,m ; register float sum1,sum2,sum3 , *av ;
float *xmat , *u1,*u2,*u3 , *v1,*v2,*v3 , *z1,*z2,*z3 ;
int nite ;
float g11,g21,g22,g31,g32,g33 ;
if( nn < 3 || mm < 3 || xx == NULL ) return -1.0f ;
n = nn ; m = mm ;
#undef X
#define X(i,j) xmat[(i)+(j)*n]
xmat = xx ;
u1 = (float *)calloc(sizeof(float),n) ;
u2 = (float *)malloc(sizeof(float)*n) ;
u3 = (float *)malloc(sizeof(float)*n) ;
v1 = (float *)malloc(sizeof(float)*n) ;
v2 = (float *)malloc(sizeof(float)*n) ;
v3 = (float *)malloc(sizeof(float)*n) ;
z1 = (float *)malloc(sizeof(float)*m) ;
z2 = (float *)malloc(sizeof(float)*m) ;
z3 = (float *)malloc(sizeof(float)*m) ;
/** initialize trial eigvevectors in u1,u2,u3 **/
for( jj=0 ; jj < m ; jj++ ){
av = xmat + jj*n ;
for( ii=0 ; ii < n ; ii++ ) u1[ii] += av[ii] ;
}
for( ii=0 ; ii < n ; ii++ ){
u2[ii] = drand48()-0.5 ; u3[ii] = drand48()-0.5 ;
}
#undef NORMALIZE
#define NORMALIZE(vec) \
do{ register float sss ; register int qq ; \
sss = 0.0f ; \
for( qq=0 ; qq < n ; qq++ ) sss += vec[qq]*vec[qq] ; \
sss = 1.0f / sqrtf(sss) ; \
for( qq=0 ; qq < n ; qq++ ) vec[qq] *= sss ; \
} while(0)
NORMALIZE(u1) ; NORMALIZE(u2) ; NORMALIZE(u3) ;
/*----- iteration -----*/
for( nite=1 ; nite < 999 ; nite++ ){
/** G = U' U = 3x3 matrix **/
sum1 = sum2 = sum3 = 0.0f ;
for( ii=0 ;ii < n ; ii++ ){
sum1 += u1[ii]*u2[ii] ; sum2 += u1[ii]*u3[ii] ; sum3 += u2[ii]*u3[ii] ;
}
g21 = sum1 ; g31 = sum2 ; g32 = sum3 ;
g11 = g22 = g33 = 1.0f ; /* columns are normalized */
/** Choleski factor G = L L' **/
g21 = g21 / g11 ;
g22 = sqrtf(g22 - g21*g21) ;
g31 = g31 / g11 ;
g32 = (g32 - g31*g21) / g22 ;
g33 = sqrtf(g33 - g31*g31 - g32*g32) ;
/** V = A U = X X' U = n x 3 matrix **/
for( jj=0 ; jj < m ; jj++ ){
sum1 = sum2 = sum3 = 0.0f ; av = xmat + jj*n ;
for( ii=0 ; ii < n ; ii++ ){
sum1 += av[ii] * u1[ii] ;
sum2 += av[ii] * u2[ii] ;
sum3 += av[ii] * u3[ii] ;
}
z1[jj] = sum1 ; z2[jj] = sum2 ; z3[jj] = sum3 ;
}
memset(v1,0,sizeof(float)*n) ;
memset(v2,0,sizeof(float)*n) ;
memset(v3,0,sizeof(float)*n) ;
for( jj=0 ; jj < m ; jj++ ){
sum1 = z1[jj] ; sum2 = z2[jj] ; sum3 = z3[jj] ; av = xmat + jj*n ;
for( ii=0 ; ii < n ; ii++ ){
v1[ii] += av[ii] * sum1 ;
v2[ii] += av[ii] * sum2 ;
v3[ii] += av[ii] * sum3 ;
}
}
/** H = U' V = 3x3 matrix (symmetric) **/
sum1 = sum2 = sum3 = 0.0f ;
for( ii=0 ; ii < n ; ii++ ){
sum1 += u1[ii] * v1[ii] ;
sum2 += u1[ii] * v2[ii] ;
sum3 += u1[ii] * v3[ii] ;
}
h11 = sum1 ; h12 = h21 = sum2 ; h13 = h31 = sum3 ;
sum1 = sum2 = sum3 = 0.0f ;
for( ii=0 ; ii < n ; ii++ ){
sum1 += u2[ii] * v2[ii] ;
sum2 += u2[ii] * v3[ii] ;
sum3 += u3[ii] * v3[ii] ;
}
h22 = sum1 ; h23 = h32 = sum2 ; h33 = sum3 ;
//Receive encoder ticks and send 'odom' and 'tf'
void robotDataCallback(std::string * data){
if (confirm_communication){
//ROS_INFO("Robot -- Communication OK! Received: \"%s\"", data->c_str());
ROS_INFO("Traxbot is Streaming Data.");
confirm_communication = false;
}
int first_at = data->find_first_of("@", 0);
int second_at = data->find_first_of("@", first_at+1);
int first_comma = data->find_first_of(",", 0);
int second_comma = data->find_first_of(",", first_comma+1);
//protection against broken msgs from the buffer (e.g., '@6,425@6,4250,6430e')
if ( second_at > -1 || second_comma == -1){
ROS_WARN("%s ::: ENCODER MSG IGNORED", data->c_str());
return;
}
int left_encoder_count, right_encoder_count;
sscanf(data->c_str(), "@6,%d,%de", &right_encoder_count, &left_encoder_count); //encoder msg parsing
//protection against broken msgs from the buffer (e.g., '@6,425@6,4250,6430e')
if ( abs(right_encoder_count) > MAX_ENCODER_COUNT || abs(left_encoder_count) > MAX_ENCODER_COUNT){
ROS_WARN("Encoders > MAX_ENCODER_COUNT");
return;
}
double last_x = odometry_x_;
double last_y = odometry_y_;
double last_yaw = odometry_yaw_;
// It is not necessary to reset the encoders:
int right_enc_dif = 0, left_enc_dif = 0;
if ( right_encoder_prev >= (0.75*MAX_ENCODER_COUNT) && signof (right_encoder_count) == 0 && signof(right_encoder_prev) == 1 ){
right_enc_dif = (MAX_ENCODER_COUNT-right_encoder_prev) + (MAX_ENCODER_COUNT+right_encoder_count);
}else if (right_encoder_prev <= (-0.75*MAX_ENCODER_COUNT) && signof (right_encoder_count) == 1 && signof(right_encoder_prev) == 0){
right_enc_dif = -(MAX_ENCODER_COUNT+right_encoder_prev) + (right_encoder_count-MAX_ENCODER_COUNT);
}else{
right_enc_dif = right_encoder_count - right_encoder_prev;
}
if ( left_encoder_prev >= (0.75*MAX_ENCODER_COUNT) && signof (left_encoder_count) == 0 && signof(left_encoder_prev) == 1){
left_enc_dif = (MAX_ENCODER_COUNT-left_encoder_prev) + (MAX_ENCODER_COUNT+left_encoder_count);
}else if (left_encoder_prev <= (-0.75*MAX_ENCODER_COUNT) && signof (left_encoder_count) == 1 && signof(left_encoder_prev) == 0){
left_enc_dif = -(MAX_ENCODER_COUNT+left_encoder_prev) + (left_encoder_count-MAX_ENCODER_COUNT);
}else{
left_enc_dif = left_encoder_count - left_encoder_prev;
}
//calulate Odometry:
double dist = (right_enc_dif*PULSES_TO_M + left_enc_dif*PULSES_TO_M) / 2.0;
double ang = (right_enc_dif*PULSES_TO_M - left_enc_dif*PULSES_TO_M) / AXLE_LENGTH;
bool publish_info = true;
if (right_encoder_prev == right_encoder_count && left_encoder_prev == left_encoder_count){
publish_info = false;
}
// update previous encoder counts:
left_encoder_prev = left_encoder_count;
right_encoder_prev = right_encoder_count;
// Update odometry
odometry_yaw_ = NORMALIZE(odometry_yaw_ + ang); // rad
odometry_x_ = odometry_x_ + dist*cos(odometry_yaw_); // m
odometry_y_ = odometry_y_ + dist*sin(odometry_yaw_); // m
last_time = current_time;
current_time = ros::Time::now();
// Calculate the robot speed
double dt = (current_time - last_time).toSec();
double vel_x = (odometry_x_ - last_x)/dt;
double vel_y = (odometry_y_ - last_y)/dt;
double vel_yaw = (odometry_yaw_ - last_yaw)/dt;
//Publish at least at most at 75Hz
/*if (current_time.toSec() - 0.013 >= last_time_pub){
publish_info = true;
}*/
//if(publish_info){
last_time_pub = current_time.toSec();
// Since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometry_yaw_);
// First, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = odometry_x_;
odom_trans.transform.translation.y = odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
// Send the transform
odom_broadcaster_ptr->sendTransform(odom_trans); // odom->base_link transform
// Next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
// Set the position
odom.pose.pose.position.x = odometry_x_;
odom.pose.pose.position.y = odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
// Set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
// Publish the message
odom_pub_ptr->publish(odom); // odometry publisher
//}
}
static void DrawShips(void)
{
register struct player *j;
register struct phaser *php;
char idbuf[10];
const int view = SCALE * TWINSIDE / 2;
int dx, dy, px, py, wx, wy, tx, ty, lx, ly;
#ifndef DYNAMIC_BITMAPS
W_Icon(*ship_bits)[VIEWS];
#endif
/* Kludge to try to fix missing ID chars on tactical (short range) display.
*
*/
idbuf[0] = '0';
idbuf[1] = '\0';
for (j = players + MAXPLAYER - 1; j >= players; --j)
{
#ifdef HAVE_XPM
void *sprite = S_Ship(j->p_no);
#else
if ((j->p_status != PALIVE) && (j->p_status != PEXPLODE))
continue;
if (j->p_flags & PFOBSERV)
{
/* observer and NOT locked onto a player (ie. locked onto planet or
* * * vacuum) */
if (!(j->p_flags & PFPLOCK))
continue;
/* observer and NOT cloaked - don't display ship but do tractors *
* * phasers and torps are done for that ship already */
if (!(j->p_flags & PFCLOAK))
continue;
}
#endif /* HAVE_XPM */
/* jmn - observer support.. tried to diplay tractors but no works */
if (j->p_flags & PFCLOAK)
{
if (j->p_cloakphase < (CLOAK_PHASES - 1))
{
#ifdef SOUND
if (myPlayer(j) && (j->p_cloakphase == 0)) {
#if defined(HAVE_SDL)
Play_Sound(CLOAKED_WAV);
#else
Play_Sound(CLOAK_SOUND);
#endif
}
#endif
j->p_cloakphase++;
}
}
else
{
if (j->p_cloakphase)
{
#ifdef SOUND
if (myPlayer(j))
if (j->p_cloakphase == CLOAK_PHASES - 1) {
#if defined(HAVE_SDL)
Play_Sound(UNCLOAK_WAV);
#else
Play_Sound(UNCLOAK_SOUND);
} else {
Abort_Sound(CLOAK_SOUND);
#endif
}
#endif
j->p_cloakphase--;
}
}
dx = j->p_x - me->p_x;
dy = j->p_y - me->p_y;
#ifdef HAVE_XPM
if ((sprite == NULL) && (dx > view || dx < -view || dy > view || dy < -view))
#else
if (dx > view || dx < -view || dy > view || dy < -view)
#endif
continue;
dx = dx / SCALE + TWINSIDE / 2;
dy = dy / SCALE + TWINSIDE / 2;
#ifdef HAVE_XPM
if ((sprite == NULL) || (pixFlags & NO_CLK_PIX))
#endif
if (j->p_flags & PFCLOAK && (j->p_cloakphase == (CLOAK_PHASES - 1)))
{
if (myPlayer(j)
#ifdef RECORDGAME
|| playback
#endif
)
{
W_WriteBitmap(dx - (cloak_width / 2), dy - (cloak_height / 2),
cloakicon, myColor);
#ifdef VARY_HULL
clearzone[0][clearcount] = dx - (shield_width / 2 + 1);
clearzone[1][clearcount] = dy - (shield_height / 2 + 1);
clearzone[2][clearcount] = shield_width + 3;
clearzone[3][clearcount] = shield_height + 3;
clearcount++;
#else
clearzone[0][clearcount] = dx - (shield_width / 2);
clearzone[1][clearcount] = dy - (shield_height / 2);
clearzone[2][clearcount] = shield_width;
clearzone[3][clearcount] = shield_height;
clearcount++;
#endif
goto shieldlabel; /* draw the shield even when
* * * cloaked */
}
continue;
}
if (j->p_status == PALIVE)
{
#ifndef DYNAMIC_BITMAPS
switch (j->p_team)
{
case FED:
#ifdef TNG_FED_BITMAPS
if (use_tng_fed_bitmaps)
ship_bits = tng_fed_bitmaps;
else
#endif
ship_bits = fed_bitmaps;
break;
case ROM:
ship_bits = rom_bitmaps;
break;
case KLI:
ship_bits = kli_bitmaps;
break;
case ORI:
ship_bits = ori_bitmaps;
break;
default:
ship_bits = ind_bitmaps;
break;
}
#endif
#if defined (VARY_HULL) || defined (BEEPLITE)
clearzone[0][clearcount] = dx - (shield_width / 2 + 6);
clearzone[1][clearcount] = dy - (shield_height / 2 + 6);
clearzone[2][clearcount] = shield_width + 12;
clearzone[3][clearcount] = shield_height + 12;
clearcount++;
#else
clearzone[0][clearcount] = dx - (shield_width / 2);
clearzone[1][clearcount] = dy - (shield_height / 2);
clearzone[2][clearcount] = shield_width;
clearzone[3][clearcount] = shield_height;
clearcount++;
#endif
#ifdef HAVE_XPM
if (sprite != NULL)
{
clearsize = W_DrawSprite(sprite, dx, dy, TWINSIDE);
#if defined (VARY_HULL)
if (clearsize > shield_width + 12)
#else
if (clearsize > shield_width)
#endif
{
clearcount--;
clearzone[0][clearcount] = dx - (clearsize / 2);
clearzone[1][clearcount] = dy - (clearsize / 2);
clearzone[2][clearcount] = clearsize;
clearzone[3][clearcount] = clearsize;
clearcount++;
}
}
else
#endif /* HAVE_XPM */
if (j->p_team == ROM && j->p_ship.s_type == CRUISER &&
ROMVLVS)
W_WriteBitmap(dx - (j->p_ship.s_width / 2),
dy - (j->p_ship.s_height / 2), ROMVLVS_bitmap[rosette(j->p_dir)],
playerColor(j));
else
W_WriteBitmap(dx - (j->p_ship.s_width / 2),
dy - (j->p_ship.s_height / 2),
#ifndef DYNAMIC_BITMAPS
ship_bits[j->p_ship.s_type][rosette(j->p_dir)],
#else
ship_bitmaps[PlayerBitmap(j)][rosette(j->p_dir)],
#endif
playerColor(j));
if (j->p_cloakphase > 0)
{
#ifdef HAVE_XPM
if (sprite == NULL)
#endif
W_WriteBitmap(dx - (cloak_width / 2),
dy - (cloak_height / 2), cloakicon, playerColor(j));
if (!myPlayer(j)) /* if myplayer draw the shield */
continue;
}
shieldlabel:
#ifdef BEEPLITE
if ((UseLite && emph_player_seq_n[j->p_no] > 0)
&& (liteflag & LITE_PLAYERS_LOCAL))
{
int seq_n = emph_player_seq_n[j->p_no] % emph_player_seql_frames;
W_WriteBitmap(dx - (emph_player_seql_width / 2),
dy - (emph_player_seql_height / 2),
emph_player_seql[seq_n],
W_White);
}
#endif
#ifdef VARY_HULL
if (j == me && vary_hull)
{
int hull_left = (100 * (me->p_ship.s_maxdamage -
me->p_damage)) / me->p_ship.s_maxdamage, hull_num = 7;
int hull_color;
if (hull_left <= 16)
{
hull_num = 0;
hull_color = W_Red;
}
else if (hull_left <= 28)
{
hull_num = 1;
hull_color = W_Yellow;
}
else if (hull_left <= 40)
{
hull_num = 2;
hull_color = W_Green;
}
else if (hull_left <= 52)
{
hull_num = 3;
hull_color = W_Green;
}
else if (hull_left <= 64)
{
hull_num = 4;
hull_color = W_Green;
}
else if (hull_left <= 76)
{
hull_num = 5;
hull_color = W_White;
}
else if (hull_left <= 88)
{
hull_num = 6;
hull_color = W_White;
}
else
hull_color = playerColor(j);
W_WriteBitmap(dx - (shield_width / 2 + 1),
dy - (shield_height / 2 + 1),
hull[hull_num], hull_color);
}
#endif
#ifdef SOUND
if (j->p_no == me->p_no)
{
if ((sound_flags & PFSHIELD) && !(j->p_flags & PFSHIELD)) {
#if defined(HAVE_SDL)
Play_Sound(SHIELD_DOWN_WAV);
#else
Play_Sound(SHIELD_DOWN_SOUND);
#endif
}
if (!(sound_flags & PFSHIELD) && (j->p_flags & PFSHIELD)) {
#if defined(HAVE_SDL)
Play_Sound(SHIELD_UP_WAV);
#else
Play_Sound(SHIELD_UP_SOUND);
#endif
}
}
#endif
/* It used to be that if "showShields" was false, shields were not
* * * shown. COW has already stopped accepting showShields flags
* * from * the server, now stop using showShields altogether.
* James * Soutter * (Zork) 4 Jan 95 */
if (j->p_flags & PFSHIELD)
{
int color = playerColor(j);
#ifdef VSHIELD_BITMAPS
int shieldnum;
if (j == me && VShieldBitmaps)
{
shieldnum = SHIELD_FRAMES * me->p_shield / me->p_ship.s_maxshield;
if (shieldnum >= SHIELD_FRAMES)
shieldnum = SHIELD_FRAMES - 1;
color = gColor;
if (shieldnum < SHIELD_FRAMES * 2 / 3)
{
color = yColor;
if (shieldnum < SHIELD_FRAMES * 2 / 3)
{
color = rColor;
}
}
}
else
{
color = playerColor(j);
shieldnum = 2;
}
#endif
if (warnShields && j == me)
{
switch (me->p_flags & (PFGREEN | PFYELLOW | PFRED))
{
case PFGREEN:
color = gColor;
break;
case PFYELLOW:
color = yColor;
break;
case PFRED:
color = rColor;
break;
}
}
#ifdef VSHIELD_BITMAPS
W_WriteBitmap(dx - (shield_width / 2),
dy - (shield_height / 2), shield[shieldnum], color);
#else
W_WriteBitmap(dx - (shield_width / 2),
dy - (shield_height / 2), shield, color);
#endif
}
/* Det circle */
if (detCircle)
{
if (myPlayer(j) || isObsLockPlayer(j))
{
int dcr = DETDIST*2/SCALE;
int dcx = dx - (dcr/2);
int dcy = dy - (dcr/2);
W_WriteCircle(w, dcy, dcy, dcr, W_Red);
clearzone[0][clearcount] = dcx;
clearzone[1][clearcount] = dcy;
clearzone[2][clearcount] = dcr + dcr;
clearzone[3][clearcount] = dcr + dcr;
clearcount++;
detCircle--;
}
}
if (j->p_flags & PFCLOAK) /* when cloaked stop here */
continue;
{
int color = playerColor(j);
idbuf[0] = *(shipnos + j->p_no);
if (myPlayer(j) || isObsLockPlayer(j))
{
switch (me->p_flags & (PFGREEN | PFYELLOW | PFRED))
{
case PFGREEN:
color = gColor;
break;
case PFYELLOW:
color = yColor;
break;
case PFRED:
color = rColor;
break;
}
}
W_MaskText(w, dx + (j->p_ship.s_width / 2),
dy - (j->p_ship.s_height / 2), color,
idbuf, 1, shipFont(j));
clearzone[0][clearcount] = dx + (j->p_ship.s_width / 2);
clearzone[1][clearcount] = dy - (j->p_ship.s_height / 2);
clearzone[2][clearcount] = W_Textwidth;
clearzone[3][clearcount] = W_Textheight;
clearcount++;
}
}
else if (j->p_status == PEXPLODE)
{
int i;
i = j->p_explode;
#ifdef SOUND
if (i == 1)
#if defined(HAVE_SDL)
Play_Sound(j == me ? EXPLOSION_WAV : EXPLOSION_OTHER_WAV);
#else
Play_Sound(j == me ? EXPLOSION_SOUND : OTHER_EXPLOSION_SOUND);
#endif
#endif
#ifdef HAVE_XPM
if (sprite != NULL)
{
clearsize = W_DrawSprite(sprite, dx, dy, TWINSIDE);
clearzone[0][clearcount] = dx - (clearsize / 2);
clearzone[1][clearcount] = dy - (clearsize / 2);
clearzone[2][clearcount] = clearsize;
clearzone[3][clearcount] = clearsize;
clearcount++;
}
else
#endif
if (i < EX_FRAMES ||
(i < SBEXPVIEWS && j->p_ship.s_type == STARBASE))
{
if (j->p_ship.s_type == STARBASE)
{
W_WriteBitmap(dx - (sbexp_width / 2),
dy - (sbexp_height / 2), sbexpview[i],
playerColor(j));
clearzone[0][clearcount] = dx - (sbexp_width / 2);
clearzone[1][clearcount] = dy - (sbexp_height / 2);
clearzone[2][clearcount] = sbexp_width;
clearzone[3][clearcount] = sbexp_height;
}
else
{
W_WriteBitmap(dx - (ex_width / 2), dy - (ex_height / 2),
expview[i], playerColor(j));
clearzone[0][clearcount] = dx - (ex_width / 2);
clearzone[1][clearcount] = dy - (ex_height / 2);
clearzone[2][clearcount] = ex_width;
clearzone[3][clearcount] = ex_height;
}
clearcount++;
j->p_explode++;
}
}
/* Now draw his phaser (if it exists) */
php = &phasers[j->p_no];
if (php->ph_status != PHFREE)
{
#ifdef SOUND
if (!sound_phaser)
{
#if defined(HAVE_SDL)
Play_Sound(j == me ? PHASER_WAV : PHASER_OTHER_WAV);
#else
Play_Sound(j == me ? PHASER_SOUND : OTHER_PHASER_SOUND);
#endif
sound_phaser++;
}
#endif
if ((php->ph_updateFuse -= weaponUpdate) == 0)
{
/* Expire the phaser */
#if 0
fputs("[phaser]", stderr);
fflush(stderr);
#endif
php->ph_status = PHFREE;
}
else
{
if (php->ph_status == PHMISS)
{
/* Here I will have to compute end coordinate */
tx = PHASEDIST * j->p_ship.s_phaserdamage / 100 *
Cos[php->ph_dir];
ty = PHASEDIST * j->p_ship.s_phaserdamage / 100 *
Sin[php->ph_dir];
tx = (j->p_x + tx - me->p_x) / SCALE + TWINSIDE / 2;
ty = (j->p_y + ty - me->p_y) / SCALE + TWINSIDE / 2;
php->ph_fuse = 0;
}
else if (php->ph_status == PHHIT2)
{
tx = (php->ph_x - me->p_x) / SCALE + TWINSIDE / 2;
ty = (php->ph_y - me->p_y) / SCALE + TWINSIDE / 2;
}
else
{ /* Start point is dx, dy */
tx = (players[php->ph_target].p_x - me->p_x) /
SCALE + TWINSIDE / 2;
ty = (players[php->ph_target].p_y - me->p_y) /
SCALE + TWINSIDE / 2;
}
/* Shrink the phasers if necessary: * * Measure length in 16ths
*
* * to make the maths a little * easier for the computer (div
* 16 * is a 4 bit shift). * * Add 8 to each sum to round
* properly. */
if (shrinkPhaserOnMiss || php->ph_status != PHMISS)
{
if (j == me)
{
px = (dx * (16 - phaserShrink) + tx * phaserShrink + 8)
/ 16;
py = (dy * (16 - phaserShrink) + ty * phaserShrink + 8)
/ 16;
}
else
{
px = (dx * (16 - theirPhaserShrink) +
tx * theirPhaserShrink + 8) / 16;
py = (dy * (16 - theirPhaserShrink) +
ty * theirPhaserShrink + 8) / 16;
}
}
else
{
px = dx;
py = dy;
}
/* Now draw the phasers */
if (friendlyPlayer(j))
{
if (highlightFriendlyPhasers && (php->ph_status == PHHIT))
W_CacheLine(w, px, py, tx, ty, foreColor);
else
{
if ((php->ph_fuse % 2) == 1)
W_CacheLine(w, px, py, tx, ty, foreColor);
else
W_CacheLine(w, px, py, tx, ty, shipCol[remap[j->p_team]]);
}
php->ph_fuse++;
clearline[0][clearlcount] = px;
clearline[1][clearlcount] = py;
clearline[2][clearlcount] = tx;
clearline[3][clearlcount] = ty;
clearlcount++;
}
else
{
if ((enemyPhasers > 0) && (enemyPhasers <= 10))
{
unsigned char dir;
if (tx == px && ty == py)
continue;
#ifdef SHORT_PACKETS
if (php->ph_status != PHMISS) /* KOC 10/20/95 */
{ /* hack for SP_2 */
#define XPI 3.1415926
dir = (unsigned char) nint(atan2((double) (ty - py),
(double) (tx - px)) / XPI * 128.0);
#undef XPI
}
else
#endif
{
dir = NORMALIZE(php->ph_dir + 64);
}
wx = px + enemyPhasers * Cos[dir];
wy = py + enemyPhasers * Sin[dir];
lx = px - enemyPhasers * Cos[dir];
ly = py - enemyPhasers * Sin[dir];
W_MakePhaserLine(w, wx, wy, tx, ty,
shipCol[remap[j->p_team]]);
W_MakePhaserLine(w, lx, ly, tx, ty,
shipCol[remap[j->p_team]]);
php->ph_fuse++;
clearline[0][clearlcount] = wx;
clearline[1][clearlcount] = wy;
clearline[2][clearlcount] = tx;
clearline[3][clearlcount] = ty;
clearlcount++;
clearline[0][clearlcount] = lx;
clearline[1][clearlcount] = ly;
clearline[2][clearlcount] = tx;
clearline[3][clearlcount] = ty;
clearlcount++;
}
else
{
W_MakePhaserLine(w, px, py, tx, ty,
shipCol[remap[j->p_team]]);
php->ph_fuse++;
clearline[0][clearlcount] = px;
clearline[1][clearlcount] = py;
clearline[2][clearlcount] = tx;
clearline[3][clearlcount] = ty;
clearlcount++;
}
}
}
}
#ifdef SOUND
else if (j->p_no == me->p_no)
sound_phaser = 0;
#endif
/* ATM - show tractor/pressor beams (modified by James Collins) */
/* showTractorPressor is a variable set by xtrekrc. */
if (showTractorPressor)
{
if (myPlayer(j) && isAlive(me) && (j->p_flags & PFTRACT || j->p_flags & PFPRESS))
{
double theta;
unsigned char dir;
int lx[2], ly[2], target_width;
struct player *tractee;
if (me->p_tractor < 0 || me->p_tractor >= MAXPLAYER)
continue;
tractee = &players[me->p_tractor];
if (tractee->p_status != PALIVE ||
((tractee->p_flags & PFCLOAK) &&
(tractee->p_cloakphase == (CLOAK_PHASES - 1))))
{
continue;
}
if (tcounter >= 2)
{ /* continue tractor stuff */
if (!continuetractor)
tcounter--;
px = (players[me->p_tractor].p_x - me->p_x)
/ SCALE + TWINSIDE / 2;
py = (players[me->p_tractor].p_y - me->p_y)
/ SCALE + TWINSIDE / 2;
if (px == dx && py == dy)
continue; /* this had better be last *
*
* * in for(..) */
#define XPI 3.1415926
theta = atan2((double) (px - dx),
(double) (dy - py)) + XPI / 2.0;
dir = (unsigned char) nint(theta / XPI * 128.0);
if (tractee->p_flags & PFSHIELD)
target_width = shield_width;
else
{
target_width = tractee->p_ship.s_width / 2;
}
lx[0] = px + (Cos[dir] * (target_width / 2));
ly[0] = py + (Sin[dir] * (target_width / 2));
lx[1] = px - (Cos[dir] * (target_width / 2));
ly[1] = py - (Sin[dir] * (target_width / 2));
#undef XPI
if (j->p_flags & PFPRESS)
{
W_MakeTractLine(w, dx, dy, lx[0], ly[0], W_Yellow);
W_MakeTractLine(w, dx, dy, lx[1], ly[1], W_Yellow);
}
else
{
W_MakeTractLine(w, dx, dy, lx[0], ly[0], W_Green);
W_MakeTractLine(w, dx, dy, lx[1], ly[1], W_Green);
}
#ifdef WIN32
/* Fixup for minor inconsistencies between SAC's interger *
*
* * linedraw and Win32 LineTo() */
tpline = clearlcount;
#endif
clearline[0][clearlcount] = dx;
clearline[1][clearlcount] = dy;
clearline[2][clearlcount] = lx[0];
clearline[3][clearlcount] = ly[0];
clearlcount++;
clearline[0][clearlcount] = dx;
clearline[1][clearlcount] = dy;
clearline[2][clearlcount] = lx[1];
clearline[3][clearlcount] = ly[1];
clearlcount++;
}
}
else if (!(me->p_flags & PFPRESS || me->p_flags & PFTRACT))
tcounter = 2;
}
}
}
int fwide(FILE *f, int mode)
{
if (!f->mode) f->mode = NORMALIZE(mode);
return f->mode;
}
