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