// ==============================================================================================================================================
// Translate TrL from one orbit to an other co-planar orbit
//
double Orbit::Translate(Orbit *tgt, double longitude)
{
VECTOR3 lv=Position(longitude);
double xf=dotp(lv,tgt->minv);
double xl=dotp(lv,tgt->majv);
return limit(atan2(xf,xl)+tgt->lpe);
}
float det(void) const
{
fvec3 c1 = cols[0], c2 = cols[1], c3 = cols[2];
return dotp(c3.shuffle<2, 1, 0>(), c2.shuffle<1, 0, 2>() * c1.shuffle<0, 2, 1>()) -
dotp(c1, c2.shuffle<2, 0, 1>() * c3.shuffle<1, 2, 0>());
}
// ==============================================================================================================================================
// pr = unit vector[in], rad[out]=orbital radius, height[out]=out of plain angle?, angle[out]=true longitude
//
void Orbit::Longitude(VECTOR3 pr, double *rad, double *height, double *angle)
{
double xf=dotp(pr,minv);
double xl=dotp(pr,majv);
if (rad) *rad = sqrt((xl*xl) + (xf*xf));
if (height) *height= dotp(pr,norv);
if (angle) *angle = limit(atan2(xf,xl)+lpe);
}
void
SphereMapping(
Mapping *map,
Geom * /*obj*/,
Vector *pos,
Vector * /*norm*/,
Vec2d *uv,
Vector * /*dpdu*/,
Vector * /*dpdv*/)
{
Vector vtmp;
Float nx, ny, nz, phi, theta;
VecSub(*pos, map->center, &vtmp);
if (VecNormalize(&vtmp) == 0.) {
/*
* Point is coincident with origin of sphere. Punt.
*/
uv->u = uv->v = 0.;
return;
}
/*
* Find location of point projected onto unit sphere
* in the sphere's coordinate system.
*/
nx = dotp(&map->uaxis, &vtmp);
ny = dotp(&map->vaxis, &vtmp);
nz = dotp(&map->norm, &vtmp);
if (nz > 1.) /* roundoff */
phi = PI;
else if (nz < -1.)
phi = 0;
else
phi = acos(-nz);
uv->v = phi / PI;
if (fabs(uv->v) < EPSILON || equal(uv->v, 1.))
uv->u = 0.;
else {
theta = nx / sin(phi);
if (theta > 1.)
theta = 0.;
else if (theta < -1.)
theta = 0.5;
else
theta = acos(theta) / TWOPI;
if (ny > 0)
uv->u = theta;
else
uv->u = 1 - theta;
}
}
/* Calculate the Jacobian for a given stream coordinate */
double Jacob(const double* a, const double* b, double sint, double xp, int verb)
{
double aa, bb, abn, tmp, j;
aa = dotp(a, a);
bb = dotp(b, b);
abn = norm(a) * norm(b);
tmp = aa * sint * sint + bb * ((1 - sint * sint) * (1 - sint * sint));
j = sqrt(tmp) + (abn * xp) / tmp;
return fabs(j);
}
// ==============================================================================================================================================
//
void Orbit::GDIDrawLineOfNodes(HDC hDC, double width, double height, double hpa, DWORD color, double cx, double cy, double zoom, bool box, bool line)
{
double x,y,ox,oy;
double hpd = limit(hpa+PI);
VECTOR3 pos;
double w = width;
double h = height;
HPEN pen=CreatePen(PS_DOT,1,color);
if (length(displacement)>0) {
Point(displacement,cx,cy,zoom,&x,&y);
cx=x, cy=y;
}
pos = Position(hpa);
x = cx + (dotp(pos,zero) * zoom);
y = cy - (dotp(pos,nine) * zoom);
pos = Position(hpd);
ox = cx + (dotp(pos,zero) * zoom);
oy = cy - (dotp(pos,nine) * zoom);
SelectObject(hDC,pen);
if (line) DrawLine(hDC,x,y,ox,oy,w,h,false);
if (box) {
double r=3.0;
HPEN spen=CreatePen(PS_SOLID,1,color);
HBRUSH brush=CreateSolidBrush(color);
HBRUSH brushb=CreateSolidBrush(0);
SelectObject(hDC,spen);
SelectObject(hDC,brush);
DrawRectangle(hDC,x-r,y-r,x+r,y+r,w,h);
SelectObject(hDC,brushb);
DrawRectangle(hDC,ox-r,oy-r,ox+r,oy+r,w,h);
DeleteObject(spen);
DeleteObject(brush);
DeleteObject(brushb);
}
SelectObject(hDC,GetStockObject(NULL_BRUSH));
SelectObject(hDC,GetStockObject(WHITE_PEN));
DeleteObject(pen);
}
// ==============================================================================================================================================
// Define planet excape orbit, Pos = vessel position, Esc = Escape vector,
//
void Orbit::EscapeOrbit(OBJHANDLE ref,VECTOR3 Pos,VECTOR3 Esc,VECTOR3 normal)
{
double myy=GC*oapiGetMass(ref);
double rad=length(Pos);
double Esc2=dotp(Esc,Esc);
double ang=nangle(Pos,Esc,normal);
double sma = -myy / Esc2; // (constant)
double sq=sin(ang);
double q=rad*rad*sq*sq;
double w=2.0*sma*rad*(cos(ang)-1.0);
double ecc=sqrt((q-rad*sq*sqrt(q+2.0*w)+w)/(2.0*sma*sma)+1.0);
double par=sma*(1.0-ecc*ecc);
// Compute Tangential angle
double x=(par-rad)/(rad*ecc);
if (x>1) x=1; if (x<-1) x=-1;
double tra=acos(x);
double y=PI+acos(1.0/ecc);
if (ang<y) tra=PI2-tra;
double ta=tra2gamma(tra,ecc);
VECTOR3 Vel=create_vector(normal,Pos,ta);
Vel=set_length(Vel,sqrt(2*myy/rad - myy/sma));
Elements(Pos,Vel,myy);
}
int main() {
// Generate input vectors
double *x = gen_array(ARRAY_SIZE);
double *y = gen_array(ARRAY_SIZE);
double serial_result = 0.0;
double result;
// calculate result serially
for(int i=0; i<ARRAY_SIZE; i++)
serial_result += x[i] * y[i];
// Test framework that sweeps the number of threads and times each ru
double start_time, run_time;
int num_threads = omp_get_max_threads();
for(int i=1; i<=num_threads; i++) {
omp_set_num_threads(i);
start_time = omp_get_wtime();
for(int j=0; j<REPEAT; j++)
result = dotp(x,y);
run_time = omp_get_wtime() - start_time;
printf(" %d thread(s) took %f seconds\n",i,run_time);
// verify result is correct (within some threshold)
if (fabs(serial_result - result) > 0.001) {
printf("Incorrect result!\n");
return -1;
}
}
return 0;
}
REAL AEC::nlms_pw(REAL mic, REAL spk, int update)
{
REAL d = mic; // desired signal
x[j] = spk;
xf[j] = Fx.highpass(spk); // pre-whitening of x
// calculate error value
// (mic signal - estimated mic signal from spk signal)
REAL e = d - dotp(w, x + j);
REAL ef = Fe.highpass(e); // pre-whitening of e
// optimize: iterative dotp(xf, xf)
dotp_xf_xf += (xf[j]*xf[j] - xf[j+NLMS_LEN-1]*xf[j+NLMS_LEN-1]);
if (update) {
// calculate variable step size
REAL mikro_ef = Stepsize * ef / dotp_xf_xf;
// update tap weights (filter learning)
int i;
for (i = 0; i < NLMS_LEN; i += 2) {
// optimize: partial loop unrolling
w[i] += mikro_ef*xf[i+j];
w[i+1] += mikro_ef*xf[i+j+1];
}
}
if (--j < 0) {
// optimize: decrease number of memory copies
j = NLMS_EXT;
memmove(x+j+1, x, (NLMS_LEN-1)*sizeof(REAL));
memmove(xf+j+1, xf, (NLMS_LEN-1)*sizeof(REAL));
}
return e;
}
// Amiga version
TTABLE *texture_init(LONG arg0)
{
WORD vers, fp;
vers = arg0 >> 16; // parse out the two arguments
fp = arg0 & 0xffff;
#else
// PC version
TTABLE *texture_init(int vers, int fp)
{
#endif
if(vers != 0x60 || !fp) // look for wrong version number and integer version
return 0L;
// called from the floating point version
ttable.work = fwork;
ttable.params = (APTR)fparams;
ttable.tform = (APTR)ftform;
return &ttable;
}
/*************
* FUNCTION: fwork
* DESCRIPTION: this is the floating point work function
* INPUT: params pointer to user definable numbers in requester
* pt pointer to patch structure - defined above
* v hit position - relative to texture axis
* t info about texture axis (TFORM array - 15 floats)
* OUTPUT: pointer to texture, NULL if non found
*************/
void fwork(FRACT *params, PATCH *pt, VECTOR *v, FRACT *t)
{
VECTOR norm;
float d;
norm.x = pt->ptc_nor.x;
norm.z = pt->ptc_nor.y;
norm.y = pt->ptc_nor.z;
// d = dotp(&pt->ptc_nor,&pt->ptc_ray[1]);
d = fabs(dotp(&norm,&pt->ptc_ray[1]));
if(d<.3)
{
pt->ptc_col[0] = 0;
pt->ptc_col[1] = 0;
pt->ptc_col[2] = 0;
}
else
{
if(d<.7)
{
pt->ptc_col[0] *= .5;
pt->ptc_col[1] *= .5;
pt->ptc_col[2] *= .5;
}
}
}
static void
PlaneUV(GeomRef gref, Vector *pos,Vector *norm, Vec2d *uv,Vector *dpdu,Vector *dpdv)
{
Plane *plane = (Plane*)gref;
Vector vec, du, dv;
VecCoordSys(norm, &du, &dv);
VecSub(*pos, plane->pos, &vec);
uv->u = dotp(&vec, &du);
uv->v = dotp(&vec, &dv);
if (dpdu)
*dpdu = du;
if (dpdv)
*dpdv = dv;
}
/*************
* DESCRIPTION: Apply RayStorm texture to surface
* INPUT: norm normal
* surf surface
* pos_ current position on surface
* ray actual ray
* OUTPUT: none
*************/
void RAYSTORM_TEXTURE::Apply(VECTOR *norm, SURFACE *surf, const VECTOR *pos_, RAY *ray)
{
TEXTURE_PATCH ptch;
VECTOR relpos,v;
ptch.ambient = surf->ambient;
ptch.specular = surf->specular;
ptch.diffuse = surf->diffuse;
ptch.reflect = surf->reflect;
ptch.transpar = surf->transpar;
ptch.difftrans = surf->difftrans;
ptch.spectrans = surf->spectrans;
ptch.refphong = surf->refphong;
ptch.transphong = surf->transphong;
ptch.foglength = surf->foglength;
ptch.refrindex = surf->refrindex;
ptch.translucency = surf->translucency;
ptch.norm = *norm;
// update texture if neccassary
if(actor && (time != ray->time))
Update(ray->time);
VecSub(pos_, &pos, &v);
relpos.x = dotp(&orient_x, &v) / size.x;
relpos.y = dotp(&orient_y, &v) / size.y;
relpos.z = dotp(&orient_z, &v) / size.z;
tinfo->work(tinfo, data, &ptch, &relpos);
surf->ambient = ptch.ambient;
surf->specular = ptch.specular;
surf->diffuse = ptch.diffuse;
surf->reflect = ptch.reflect;
surf->transpar = ptch.transpar;
surf->difftrans = ptch.difftrans;
surf->spectrans = ptch.spectrans;
surf->refphong = ptch.refphong;
surf->transphong = ptch.transphong;
surf->foglength = ptch.foglength;
surf->refrindex = ptch.refrindex;
surf->translucency = ptch.translucency;
*norm = ptch.norm;
}
int main(int argc, char** argv)
{
// Parse cmd line args.
int pointCount = 0;
if(argc == 2) {
pointCount = atoi(argv[1]);
}
else {
printf("usage: quickhull <points>\n");
exit(1);
}
long* x1 = malloc(pointCount * sizeof(long));
long* y1 = malloc(pointCount * sizeof(long));
long* x2 = malloc(pointCount * sizeof(long));
long* y2 = malloc(pointCount * sizeof(long));
long* out = malloc(pointCount * sizeof(long));
for (int i = 0; i < pointCount; i++) {
x1[i] = i;
y1[i] = i;
x2[i] = i;
y2[i] = i;
}
// Timing setup
struct timeval start, finish;
struct rusage start_ru, finish_ru;
gettimeofday( &start, NULL );
getrusage( RUSAGE_SELF, &start_ru );
// Do the deed.
dotp(pointCount, x1, y1, x2, y2, out);
// Print how long it took.
gettimeofday( &finish, NULL );
getrusage( RUSAGE_SELF, &finish_ru );
// printf("depth = %d\n", depth);
// printf("points on hull = %d\n", hull->length);
sub_timeval( &finish, &start );
sub_timeval( &finish_ru.ru_utime, &start_ru.ru_utime );
sub_timeval( &finish_ru.ru_stime, &start_ru.ru_stime );
add_timeval( &finish_ru.ru_utime, &finish_ru.ru_stime );
printf("elapsedTimeMS = ");
print_timeval( &finish ); putchar( '\n' );
printf("cpuTimeMS = ");
print_timeval( &finish_ru.ru_utime); putchar( '\n' );
}
double DockingProbe::CollisionDetection(VECTOR3 prbP, VECTOR3 prbD, VECTOR3 drgV, VECTOR3 drgA)
{
//This Function will check to see if collision has occured and
//return a vector of resultant forces if collision has occured
//If returns 0's, no collision occured.
double t = 0.0;
VECTOR3 Del;
double c0,c1,c2;
MATRIX3 M;
M = mul(drgA, drgA) - eyemat(pow(cos(DOCKINGPROBE_DROGUE_ANGLE*PI/180),2));
Del = prbP - drgV;
c0 = dotp(mul(M,Del),Del);
c1 = dotp(mul(M,prbD),Del);
c2 = dotp(mul(M,prbD),prbD);
if (c2 != 0){
double delta = c1*c1 - c0*c2;
if (delta < 0) { // no real roots
return 0.0;
} else if (delta == 0) { // tangent to cone
return 0.0;
} else { // two distinct real roots!
double t1 = (-c1 + pow(delta,0.5))/c2;
double t2 = (-c1 - pow(delta,0.5))/c2;
if (dotp(drgA, ((prbP + prbD * t1) - drgV)) > 0) { // Check for proper cone solution
t = t1;
} else {
t = t2;
}
}
}
// TODO: make sure this is checking properly
if (t <= 0 && t >= DOCKINGPROBE_PROBE_LENGTH) return 0.0; // if intersection does not occur in actual probe, do nothing
return t;
}
// ==============================================================================================================================================
//
double Orbit::TrlOfNode(VECTOR3 tgt_norv)
{
if (Defined()) {
VECTOR3 z; z.x=0; z.z=0; z.y=1.0;
VECTOR3 vlan,vect;
if (length(crossp(z,norv))==0) vlan=majv, vect=minv;
else {
vlan = unit(crossp(z,norv));
vect = unit(crossp(norv,vlan));
}
VECTOR3 lv = crossp(norv, tgt_norv);
double xf=dotp(lv,vect);
double xl=dotp(lv,vlan);
return limit(atan2(xf,xl)+lan);
}
return 0;
}
static int
PlaneIntersect(GeomRef gref, Ray *ray, Float mindist,Float* maxdist)
{
Plane *plane = (Plane*)gref;
Float d;
PlaneTests++;
d = dotp(&plane->norm, &ray->dir);
if (fabs(d) < EPSILON)
return FALSE;
d = (plane->d - dotp(&plane->norm, &ray->pos)) / d;
if (d > mindist && d < *maxdist) {
*maxdist = d;
PlaneHits++;
return TRUE;
}
return FALSE;
}
REAL AEC::nlms_pw(REAL mic, REAL spk, int update)
{
REAL d = mic; // desired signal
x[j] = spk;
xf[j] = Fx.highpass(spk); // pre-whitening of x
// calculate error value
// (mic signal - estimated mic signal from spk signal)
REAL e = d - dotp(w, x + j);
REAL ef = Fe.highpass(e); // pre-whitening of e
// optimize: iterative dotp(xf, xf)
dotp_xf_xf +=
(xf[j] * xf[j] - xf[j + NLMS_LEN - 1] * xf[j + NLMS_LEN - 1]);
if (update) {
// calculate variable step size
REAL mikro_ef = Stepsize * ef / dotp_xf_xf;
#ifdef UNROLLPTR
REAL *wp = w, *xfp = xf+j, *endw = w + NLMS_LEN;
for (; wp < endw; wp += LOOPSTEP, xfp += LOOPSTEP) {
// optimize: partial loop unrolling
wp[0] += mikro_ef * xfp[0];
wp[1] += mikro_ef * xfp[1];
#if LOOPSTEP == 4
wp[2] += mikro_ef * xfp[2];
wp[3] += mikro_ef * xfp[3];
#endif
}
#else
// update tap weights (filter learning)
int i;
for (i = 0; i < NLMS_LEN; i += LOOPSTEP) {
// optimize: partial loop unrolling
w[i] += mikro_ef * xf[i + j];
w[i + 1] += mikro_ef * xf[i + j + 1];
#if LOOPSTEP == 4
w[i+2] += mikro_ef * xf[i + j + 2];
w[i + 3] += mikro_ef * xf[i + j + 3];
#endif
}
#endif /* UNROLLPTR */
}
if (--j < 0) {
// optimize: decrease number of memory copies
j = NLMS_EXT;
memmove(x + j + 1, x, (NLMS_LEN - 1) * sizeof(REAL));
memmove(xf + j + 1, xf, (NLMS_LEN - 1) * sizeof(REAL));
}
return e;
}
void
CylinderMapping(
Mapping *map,
Geom * /*obj*/,
Vector *pos,
Vector * /*norm*/,
Vec2d *uv,
Vector *dpdu,
Vector *dpdv)
{
Vector vtmp;
Float nx, ny, r;
VecSub(*pos, map->center, &vtmp);
nx = dotp(&map->uaxis, &vtmp);
ny = dotp(&map->vaxis, &vtmp);
uv->v = dotp(&map->norm, &vtmp);
r = sqrt(nx*nx + ny*ny);
if (r < EPSILON) {
uv->u = 0.;
return;
}
nx /= r;
ny /= r;
if (fabs(nx) > 1.)
uv->u = 0.5;
else
uv->u = acos(nx) / TWOPI;
if (ny < 0.)
uv->u = 1. - uv->u;
if (dpdv)
*dpdv = map->norm;
if (dpdu)
(void)VecNormCross(&map->norm, pos, dpdu);
}
/*************
* DESCRIPTION: Apply imagine texture to surface
* INPUT: norm normal
* surf surface
* pos_ current position on surface
* ray actual ray
* OUTPUT: none
*************/
void IMAGINE_TEXTURE::Apply(VECTOR *norm, SURFACE *surf, const VECTOR *pos_, RAY *ray)
{
IM_PATCH patch; /* patch-structure */
VECTOR v, relpos; /* hit position relative to texture axis */
patch.ptc_pos = *pos_;
patch.ptc_nor = *norm;
patch.ptc_col = surf->diffuse;
patch.ptc_ref = surf->reflect;
patch.ptc_tra = surf->transpar;
patch.ptc_spc = surf->specular;
patch.ptc_shp = 2;
patch.ptc_shd = TRUE;
patch.ptc_ray = &ray->start;
patch.raydist = ray->lambda;
patch.foglen = 0.f;
// update texture if neccassary
if(actor && (time != ray->time))
Update(ray->time);
VecSub(pos_, &pos, &v);
relpos.x = dotp(&orient_x, &v);
relpos.y = dotp(&orient_y, &v);
relpos.z = dotp(&orient_z, &v);
#ifdef __PPC__
ppc_texture_work(texture->ttable->work, param, &patch, &relpos, (float*)(&pos));
#else
texture->ttable->work(param, &patch, &relpos, (float*)(&pos));
#endif
surf->diffuse = patch.ptc_col;
surf->reflect = patch.ptc_ref;
surf->transpar = patch.ptc_tra;
*norm = patch.ptc_nor;
}
/*************
* DESCRIPTION: Apply hyper texture to surface
* INPUT: norm normal
* surf surface
* pos_ current position on surface
* ray current ray
* OUTPUT: none
*************/
void HYPER_TEXTURE::Apply(VECTOR *norm, SURFACE *surf, const VECTOR *pos_, RAY *ray, float *density, float object_density, COLOR *color)
{
TEXTURE_PATCH ptch;
VECTOR relpos,v;
ptch.diffuse = *color;
ptch.object_density = object_density;
ptch.norm = *norm;
// update texture if neccassary
if(actor && (time != ray->time))
Update(ray->time);
VecSub(pos_, &pos, &v);
relpos.x = dotp(&orient_x, &v) / size.x;
relpos.y = dotp(&orient_y, &v) / size.y;
relpos.z = dotp(&orient_z, &v) / size.z;
tinfo->work(tinfo, data, &ptch, &relpos);
*color = ptch.diffuse;
*density = ptch.density;
*norm = ptch.norm;
}
sexpr sx_reverse (sexpr sx)
{
sexpr result = sx_end_of_list;
sexpr reverse = sx;
while (consp(reverse)) {
sexpr ncar = car (reverse);
if (dotp (ncar)) {
sexpr nresult = car(result);
result = nresult;
} else {
result = cons(ncar, result);
}
reverse = cdr (reverse);
}
return result;
}
/*
* create plane primitive
*/
Plane *
PlaneCreate(Vector *pos,Vector *norm)
{
Plane *plane;
Vector tmpnrm;
tmpnrm = *norm;
if (VecNormalize(&tmpnrm) == 0.) {
RLerror(RL_WARN, "Degenerate plane normal.\n");
return (Plane *)NULL;
}
plane = (Plane *)share_malloc(sizeof(Plane));
plane->norm = tmpnrm;
plane->pos = *pos;
plane->d = dotp(&plane->norm, pos);
return plane;
}
void AscentAP::SetLaunchAzimuth (double azimuth)
{
launch_azimuth = azimuth;
// current launch location in local planet frame
VECTOR3 pos, equ, dir, nml, ne, nd;
double lng, lat, rad;
double slng, clng, slat, clat;
double saz = sin(azimuth), caz = cos(azimuth);
OBJHANDLE hRef = vessel->GetGravityRef();
vessel->GetGlobalPos(pos);
oapiGlobalToLocal (hRef, &pos, &equ);
oapiLocalToEqu (hRef, equ, &lng, &lat, &rad);
slng = sin(lng), clng = cos(lng), slat = sin(lat), clat = cos(lat);
normalise(equ); // unit radius vector
// launch direction in local planet frame
dir = _V(-clng*slat*caz - slng*saz, clat*caz, -slng*slat*caz + clng*saz);
// normal of orbital plane in local planet frame
nml = crossp(dir, equ);
// normal of equator plane in local planet frame
ne = _V(0,1,0);
// direction of ascending node
nd = unit (crossp(nml, ne));
// orbit inclination
tgt.inc = acos(dotp(nml, ne));
// longitude of ascending node
tgt.lan = atan2(nd.z, nd.x);
// rotation matrix from equator plane to target orbit plane
double sinc = sin(tgt.inc), cinc = cos(tgt.inc);
double slan = sin(tgt.lan), clan = cos(tgt.lan);
MATRIX3 R1 = _M(1,0,0, 0,cinc,sinc, 0,-sinc,cinc);
MATRIX3 R2 = _M(clan,0,-slan, 0,1,0, slan,0,clan);
tgt.R = mul(R2,R1);
}
long AEC::nlms_pw(long d, long x_, int update)
{
x[j] = (short)x_;
xf[j] = (short)Fx.highpass(x_); // pre-whitening of x
// calculate error value
// (mic signal - estimated mic signal from spk signal)
long e = d - dotp(w, x + j);
long ef = Fe.highpass(e); // pre-whitening of e
// optimize: iterative dotp(xf, xf)
dotp_xf_xf +=
(xf[j] * xf[j] - xf[j + NLMS_LEN - 1] * xf[j + NLMS_LEN - 1]);
if (update) {
// calculate variable step size
long mikro_ef = (Stepsize * ef) / dotp_xf_xf;
// update tap weights (filter learning)
int i;
for (i = 0; i < NLMS_LEN; i += 2) {
// optimize: partial loop unrolling
w[i] += mikro_ef * xf[i + j];
w[i + 1] += mikro_ef * xf[i + j + 1];
}
}
if (--j < 0) {
// optimize: decrease number of memory copies
j = NLMS_EXT;
memmove(x + j + 1, x, (NLMS_LEN - 1) * sizeof(short));
memmove(xf + j + 1, xf, (NLMS_LEN - 1) * sizeof(short));
}
// Saturation
if (e > 32767) {
return 32767;
} else if (e < -32767) {
return -32767;
} else {
return e;
}
}
static REAL dotp_sse(REAL a[], REAL b[])
{
#ifdef __SSE__
/* This is taken from speex's inner product implementation */
int j;
REAL sum;
__m128 acc = _mm_setzero_ps();
for (j=0;j<NLMS_LEN;j+=8)
{
acc = _mm_add_ps(acc, _mm_mul_ps(_mm_load_ps(a+j), _mm_loadu_ps(b+j)));
acc = _mm_add_ps(acc, _mm_mul_ps(_mm_load_ps(a+j+4), _mm_loadu_ps(b+j+4)));
}
acc = _mm_add_ps(acc, _mm_movehl_ps(acc, acc));
acc = _mm_add_ss(acc, _mm_shuffle_ps(acc, acc, 0x55));
_mm_store_ss(&sum, acc);
return sum;
#else
return dotp(a, b);
#endif
}
/*************
* DESCRIPTION: Compute normal to triangle at pos
* INPUT: dir ray direction
* hitpos hit position
* nrm computed normal
* geon geometric triangle normal
* OUTPUT: none
*************/
void TRIANGLE::Normal(const VECTOR *dir, const VECTOR *hitpos, VECTOR *nrm, VECTOR *geon)
{
*geon = norm;
if(!vnorm)
{
*nrm = norm;
}
else
{
nrm->x = b[0]*vnorm[0].x + b[1]*vnorm[1].x + b[2]*vnorm[2].x;
nrm->y = b[0]*vnorm[0].y + b[1]*vnorm[1].y + b[2]*vnorm[2].y;
nrm->z = b[0]*vnorm[0].z + b[1]*vnorm[1].z + b[2]*vnorm[2].z;
VecNormalizeNoRet(nrm);
}
if(dotp(dir, nrm) > 0.f)
{
// flip normal
SetVector(nrm, -nrm->x, -nrm->y, -nrm->z);
SetVector(geon, -geon->x, -geon->y, -geon->z);
}
}
/*************
* DESCRIPTION: Compute normal to box at pos
* INPUT: dir direction vector
* hitpos hitposition
* nrm normal
* geon geometric normal
* OUTPUT: nrm is the normal at pos
*************/
void BOX::Normal(const VECTOR *dir, const VECTOR *hitpos, VECTOR *nrm, VECTOR *geon)
{
switch(flags & OBJECT_NORMALS)
{
case OBJECT_XNORMAL:
*geon = orient_x;
break;
case OBJECT_YNORMAL:
*geon = orient_y;
break;
case OBJECT_ZNORMAL:
*geon = orient_z;
break;
}
if(dotp(dir, geon) > 0.f)
{
// flip normal
SetVector(geon, -geon->x, -geon->y, -geon->z);
}
*nrm = *geon;
}
/*************
* DESCRIPTION: calculates angle between two vectors
* INPUT: v1, v2 Vectors (unit length!)
* OUTPUT: angle between two vectors
*************/
static float VecAngle(const VECTOR v1, const VECTOR v2)
{
float cos_theta;
cos_theta = dotp(&v1, &v2);
if (cos_theta < -1.0f)
cos_theta = PI;
else
{
if (cos_theta > 1.0f)
cos_theta = 0.f;
else
return (float)acos(cos_theta);
}
/* if (cos_theta < -1.0f)
cos_theta = -1.0f;
else
if (cos_theta > 1.0f)
cos_theta = 1.0f;
*/
return cos_theta;
}
int main() {
// Generate input vectors
double *x = gen_array(ARRAY_SIZE);
double *y = gen_array(ARRAY_SIZE);
double result, answer;
answer = oracle(x, y);
// Test framework that sweeps the number of threads and times each ru
double start_time, run_time;
int num_threads = omp_get_max_threads();
for(int i=1; i<=num_threads; i++) {
omp_set_num_threads(i);
start_time = omp_get_wtime();
for(int j=0; j<REPEAT; j++) {
result = dotp(x,y);
if (result != answer) {
printf("Incorrect dotp %f %f\n", result, answer);
return 0;
}
}
run_time = omp_get_wtime() - start_time;
printf(" %d thread(s) took %f seconds\n",i,run_time);
}
}
// --------------------------------------------------------------
// Frame pre-update
// --------------------------------------------------------------
void SolarSail::clbkPreStep (double simt, double simdt, double mjd)
{
// calculate solar pressure on steering paddles
int i;
const double paddle_area = 7812.5;
const double albedo = 2.0;
static VECTOR3 ppos[4] = {
_V(0,-550,0), _V(-550,0,0), _V(0,550,0), _V(550,0,0)
};
VECTOR3 nml;
for (i = 0; i < 4; i++) {
double phi = (paddle_rot[i]-0.5)*PI;
double sphi = sin(phi), cphi = cos(phi);
if (i%2 == 0) nml = _V(-sphi,0,cphi);
else nml = _V(0,sphi,cphi);
double f = dotp (mf, nml);
if (f < 0) {
nml = -nml;
f = -f;
}
f *= paddle_area*albedo;
AddForce (nml*f, ppos[i]);
}
}