static void average_float(t_average *x, t_floatarg f) { int i, j = 0; t_float tendency; t_float geo = 1.0; x->x_average = 0; /* put value into array */ x->x_input[x->x_inpointer] = f; /* calulate average */ for(i = 0; i < x->x_index; i++) { if(x->x_mode == 0) /* linear */ { x->x_average += x->x_input[i] * (1.0 / (float)x->x_index); } else if(x->x_mode == 1) /* geometric */ { if(x->x_input[i] == 0)x->x_input[i] = 0.001; /* need to cheat a bit... */ geo *= x->x_input[i]; if(i == x->x_index - 1) x->x_average = pow(geo, (1.0/(float)x->x_index)); } else if(x->x_mode == 2) /* weighted */ { x->x_average += x->x_input[(j + x->x_inpointer + x->x_index) % x->x_index] * (float)(x->x_index - (i + 1)); j--; /* go back in array */ /* normalise output */ if(i == x->x_index - 1) x->x_average = x->x_average / (float)normalise(x->x_index - 1); } else post("average: internal error!"); } if(++x->x_inpointer > x->x_index) { x->x_inpointer = 0; if(x->x_lastaverage < x->x_average) { tendency = 1; /* getting more */ } else if(x->x_lastaverage > x->x_average) { tendency = -1; /* getting less */ } else tendency = 0; /* nothing has changed */ outlet_float(x->x_outtendency, tendency); x->x_lastaverage = x->x_average; } outlet_float(x->x_outfloat, x->x_average); }

void uade_effect_run(struct uade_effect *ue, int16_t * samples, int frames) { if (ue->enabled & (1 << UADE_EFFECT_ALLOW)) { if( ue->enabled & (1 << UADE_EFFECT_NORMALISE)) normalise(1, samples, frames); if (ue->enabled & (1 << UADE_EFFECT_PAN)) pan(ue->pan, samples, frames); if (ue->enabled & (1 << UADE_EFFECT_HEADPHONES)) headphones(samples, frames); if (ue->enabled & (1 << UADE_EFFECT_HEADPHONES2) && ue->rate) headphones2(samples, frames); if (ue->enabled & (1 << UADE_EFFECT_GAIN)) gain(ue->gain, samples, frames); } }

void upperbound(int* val) { // Lowerbound and increment the last valid one for (int i = 0; i < 6; ++i) if (val[i] == -1) val[i] = (i == 1 || i == 2) ? 1 : 0; else if (i != 5 && val[i+1] != -1) ; else ++val[i]; // Then decrement the second, to get an inclusive range --val[5]; // Normalise the result normalise(val); }

RangePrivate::RangePrivate(int &row1, int &column1, bool rowStatic1, bool colStatic1, int &row2, int &column2, bool rowStatic2, bool colStatic2, Range *parent) : q_ptr(parent), mTop(row1), mBottom(row2), mLeft(column1), mRight(column2), bRowStatic1(rowStatic1), bRowStatic2(rowStatic2), bColStatic1(colStatic1), bColStatic2(colStatic2) { normalise(); }

/* find a target in the tree -- not quick! */ virt_dirent_t * virtdir_find_tgt(virtdir_t *tp, const char *tgt, size_t tgtlen) { /* we don't need no stinking binary searches */ char path[MAXPATHLEN]; int i; (void) normalise(tgt, tgtlen, path, sizeof(path)); for (i = 0 ; i < tp->c ; i++) { if (tp->v[i].tgt && strcmp(tp->v[i].tgt, path) == 0) { return &tp->v[i]; } } return NULL; }

Quaternion& Quaternion::rotationFromTo(const Vector3& from, const Vector3& to) { // Based on Stan Melax's article in Game Programming Gems // Copy, since cannot modify local Vector3 v0 = from; Vector3 v1 = to; v0.normalize(); v1.normalize(); const FLOAT32 d = v0.dotProduct(v1); if (d >= 1.0f) // If dot == 1, vectors are the same { return makeIdentity(); } else if (d <= -1.0f) // exactly opposite { Vector3 axis(1.0f, 0.f, 0.f); axis = axis.crossProduct(v0); if (axis.length() == 0) { axis.set(0.f, 1.f, 0.f); axis = axis.crossProduct(v0); } // same as fromAngleAxis(PI, axis).normalize(); set(axis.x, axis.y, axis.z, 0); normalise(); return *this; } const FLOAT32 s = sqrtf((1 + d) * 2); // optimize inv_sqrt const FLOAT32 invs = 1.f / s; const Vector3 c = v0.crossProduct(v1)*invs; set(c.x, c.y, c.z, s * 0.5f); normalise(); return *this; }

/** * im_histplot: * @in: input image * @out: output image * * Plot a 1 by any or any by 1 image file as a max by any or * any by max image using these rules: * * <emphasis>unsigned char</emphasis> max is always 256 * * <emphasis>other unsigned integer types</emphasis> output 0 - maxium * value of @in. * * <emphasis>signed int types</emphasis> min moved to 0, max moved to max + min. * * <emphasis>float types</emphasis> min moved to 0, max moved to any * (square output) * * See also: im_hist_indexed(), im_histeq(). * * Returns: 0 on success, -1 on error */ int im_histplot( IMAGE *in, IMAGE *out ) { IMAGE *t1; if( im_check_hist( "im_histplot", in ) ) return( -1 ); if( !(t1 = im_open_local( out, "im_histplot:1", "p" )) || normalise( in, t1 ) || plot( t1, out ) ) return( -1 ); return( 0 ); }

vec3 colourHash(const std::string& str) { int hash = stringHash(str); if(hash == 0) hash++; int r = (hash/7) % 255; if(r<0) r=0; int g = (hash/3) % 255; if(g<0) g=0; int b = hash % 255; vec3 colour = normalise(vec3(r, g, b)); return colour; }

/*! Sets this quaternion by converting the rotation from angle-axis formate. The angle is specified in radians. */ void Quaternion::set(const double& angle, const vec3& axis) { // assert: axis[] is unit length // // The quaternion representing the rotation is // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) double halfAngle = 0.5*angle; double sn = sin(halfAngle); w = cos(halfAngle); x = sn*axis.x(); y = sn*axis.y(); z = sn*axis.z(); normalise(); }

void instrument_counterexamples(refactor_programt &prog, refactor_counterexamplest ces) { symbol_tablet &st=prog.st; goto_functionst &gf=prog.gf; const refactor_programt::counterexample_locationst &ce_locs= prog.counterexample_locations; const goto_programt::targetst unified(unify(ce_locs)); const std::set<irep_idt> all_keys(get_all_keys(ce_locs)); const zero_valuest zeroes(get_zero_values(st, unified)); normalise(all_keys, zeroes, ces); create_ce_arrays(st, gf, ces); create_ce_array_indexes(all_keys, st, gf); add_ce_goto(st, gf, ces.size()); assign_ce_values(st, gf, ce_locs); }

double ContactEnergy::energy() const { /* this is if we don't want to use pre-specified contacts, but rather the closest points between each link and the object all iterations. Not necessarily needed for contact energy, but useful for GQ energy */ if (mContactType == CONTACT_LIVE && mType != ENERGY_AUTOGRASP_QUALITY && mType != ENERGY_STRICT_AUTOGRASP) { mHand->getWorld()->findVirtualContacts(mHand, mObject); DBGP("Live contacts computation"); } mHand->getGrasp()->collectVirtualContacts(); //DBGP("Contact energy computation") //average error per contact VirtualContact *contact; vec3 p, n, cn; double totalError = 0; for (int i = 0; i < mHand->getGrasp()->getNumContacts(); i++) { contact = (VirtualContact *)mHand->getGrasp()->getContact(i); contact->getObjectDistanceAndNormal(mObject, &p, NULL); double dist = p.len(); //this should never happen anymore since we're never inside the object //if ( (-1.0 * p) % n < 0) dist = -dist; //BEST WORKING VERSION, strangely enough totalError += fabs(dist); //let's try this some more //totalError += distanceFunction(dist); //cn = -1.0 * contact->getWorldNormal(); //new version cn = contact->getWorldNormal(); n = normalise(p); double d = 1 - cn % n; totalError += d * 100.0 / 2.0; } totalError /= mHand->getGrasp()->getNumContacts(); //DBGP("Contact energy: " << totalError); return totalError; }

int specchord2(dataptr dz) { int exit_status; int newvc, cc, vc, n; double pre_amptotal, post_amptotal; double thisamp, thisfrq; double hifrq = dz->param[CHORD_HIFRQ]; double lofrq = dz->param[CHORD_LOFRQ]; if(dz->brksize[CHORD_HIFRQ] || dz->brksize[CHORD_LOFRQ]) { if(hifrq < lofrq) swap(&hifrq,&lofrq); } if((exit_status = get_totalamp(&pre_amptotal,dz->flbufptr[0],dz->wanted))<0) return(exit_status); for(vc = 0; vc < dz->wanted; vc += 2) dz->windowbuf[0][AMPP] = 0.0F; for( cc = 0 ,vc = 0; cc < dz->clength; cc++, vc += 2) { for(n=0;n<dz->itemcnt;n++) { /* for each note of chord */ if((exit_status = gotnewfrq2(&newvc,vc,&thisamp,&thisfrq,n,dz))<0) return(exit_status); if(exit_status==FALSE) continue; switch(dz->vflag[CHORD_BODY]) { case(CHD_LESSFUL): if((exit_status = move_data_into_appropriate_channel(vc,newvc,(float)thisamp,(float)thisfrq,dz))<0) return(exit_status); break; case(CHD_NORMAL): if((exit_status = move_data_into_some_appropriate_channel(newvc,(float)thisamp,(float)thisfrq,dz))<0) return(exit_status); break; default: sprintf(errstr,"Unknown case in dz->vflag[CHROD_BODY]: specchord()\n"); return(PROGRAM_ERROR); } } } for(vc = 0; vc < dz->wanted; vc+=2) dz->flbufptr[0][AMPP] = dz->windowbuf[0][AMPP]; if(dz->vflag[CHORD_FBOT] || dz->vflag[CHORD_FTOP]) { if((exit_status = remove_unwanted_frq_areas(lofrq,hifrq,dz))<0) return(exit_status); } if((exit_status = get_totalamp(&post_amptotal,dz->flbufptr[0],dz->wanted))<0) return(exit_status); return normalise(pre_amptotal,post_amptotal,dz); }

void RDirNode::updateSplinePoint(float dt) { if(parent == 0) return; //update the spline point vec2 td = (parent->getPos() - pos) * 0.5f; vec2 mid = pos + td;// - td.perpendicular() * pos.normal();// * 10.0; vec2 delta = (mid - spos); //dont let spos get more than half the length of the distance behind if(glm::length2(delta) > glm::length2(td)) { spos += normalise(delta) * (glm::length(delta) - glm::length(td)); } spos += delta * std::min(1.0f, dt * 2.0f); }

void interact_enemy_and_player(entity* e, rgba c) { if (player* pl = overlaps_player(e)) { if (pl->_dangerous) { fx_explosion(e->_pos, 200.0f, 40, rgba(0.0f, 1.0f), 9.0f, 20); fx_explosion(e->_pos, 300.0f, 10, c, 3.0f, 20); fx_explosion(e->_pos, 300.0f, 50, rgba(0.0f, 1.0f), 6.0f, 30); fx_explosion(e->_pos, 600.0f, 20, rgba(0.0f, 1.0f), 3.0f, 37); sound_play(sfx::UNIT_EXPLODE, 0.0f, -15.0f); vec2 d = normalise(pl->_pos - e->_pos); pl->_vel *= 0.5f; pl->_vel += d * 100.0f; destroy_entity(e); } } }

GT PFC::miller_loop(const G2& QQ,const G1& PP) { GT z; int i,j,n,nb,nbw,nzs; ECn3 A,Q; ECn P; ZZn Px,Py; BOOL precomp; ZZn6 res; Big X=*x; P=PP.g; Q=QQ.g; #ifdef MR_ECN3_PROJECTIVE Q.norm(); #endif precomp=FALSE; if (QQ.ptable!=NULL) precomp=TRUE; normalise(P); extract(P,Px,Py); Px+=Px; // because x^6+2 is irreducible.. simplifies line function calculation Py+=Py; res=1; A=Q; // reset A nb=bits(X); res.mark_as_miller(); j=0; for (i=nb-2;i>=0;i--) { res*=res; if (precomp) res*=gp(QQ.ptable,j,Px,Py); else res*=g(A,A,Px,Py); if (bit(X,i)==1) { if (precomp) res*=gp(QQ.ptable,j,Px,Py); else res*=g(A,Q,Px,Py); } } z.g=res; return z; }

void upperbound(const int* src, int* dst) { //cerr << "UBSTART " << tostring(src) << endl; // Lowerbound and increment the last valid one for (int i = 0; i < 6; ++i) if (src[i] == -1) dst[i] = (i == 1 || i == 2) ? 1 : 0; else if (i != 5 && src[i+1] != -1) dst[i] = src[i]; else dst[i] = src[i] + 1; // Then decrement the second, to get an inclusive range --dst[5]; //cerr << "UBNORM " << tostring(dst) << endl; // Normalise the result normalise(dst); }

vector<double> HMM::dhmm_em(field<rowvec> data, int max_iter) { /* * Find the ML/MAP parameters of an HMM with discrete outputs using EM. * * Notation: Q(t) = hidden state, Y(t) = observation */ //cout << "dhmm_em"<<endl; double previous_loglik = - std::numeric_limits<double>::max(); double thresh = 1e-4; double converged = 0; int num_iter = 1; int obs_prior_weight = 0; // weighting term for uniform dirichlet prior on expected emissions vector<double> LL; while ((num_iter <= max_iter) && !converged) { double loglik = 0; int S = obsmat.n_rows; int O = obsmat.n_cols; mat exp_num_trans = zeros(S,S); // all values initialized here for allocation issue (?) TODO: to be tested mat exp_num_visits1 = zeros(S,1); mat exp_num_visitsT = zeros(S,1); mat exp_num_emit = obs_prior_weight*ones(S,O); // obs_prior_weight is dirichlet in Kevin Murphy's code // E step compute_ess_dhmm(prior, transmat, obsmat, data, loglik, exp_num_trans, exp_num_visits1, exp_num_emit, exp_num_visitsT); // M step prior = normalise(exp_num_visits1); if (!exp_num_trans.is_empty()) transmat = mk_stochastic(exp_num_trans); obsmat = mk_stochastic(exp_num_emit); std::cout << "iteration " << num_iter << ", loglik = " << loglik << endl; num_iter = num_iter + 1; converged = em_converged(loglik, previous_loglik, thresh); previous_loglik = loglik; LL.push_back(loglik); } return LL; }

void FPSController::moveCamera(Ogre::Real dt) { Ogre::Real pitchAngle, pitchAngleSign; // Yaw the camera according to the mouse relative movement. cameraYawNode->yaw(rotX); // Pitch the camera according to the mouse relative movement. cameraPitchNode->pitch(rotY); // Move the camera around the world. auto normDir = accel * velocity * dt; normDir.normalise(); cameraNode->translate(cameraYawNode->getOrientation() * cameraPitchNode->getOrientation() * normDir, Ogre::Node::TS_LOCAL); // Angle of rotation around the X-Axis. pitchAngle = (2 * Ogre::Degree(Ogre::Math::ACos(cameraPitchNode->getOrientation().w)).valueDegrees()); // Just to determine the sign of the angle we pick up above, the value // itself does not interest us. pitchAngleSign = cameraPitchNode->getOrientation().x; // Limit the pitch between -90 and +90 degrees, Quake3 style. if (pitchAngle > 90.0f) { if (pitchAngleSign > 0) { // Set orientation to 90 degrees on X-Axis. cameraPitchNode->setOrientation(Ogre::Quaternion(Ogre::Math::Sqrt(0.5f), Ogre::Math::Sqrt(0.5f), 0, 0)); } else if (pitchAngleSign < 0) { // Set orientation to -90 degrees on X-Axis. cameraPitchNode->setOrientation(Ogre::Quaternion(Ogre::Math::Sqrt(0.5f), -Ogre::Math::Sqrt(0.5f), 0, 0)); } } // TODO: Terrain clamp // Notify cameraNode's children. cameraYawNode->needUpdate(); cameraPitchNode->needUpdate(); cameraRollNode->needUpdate(); // Update the camera ray. camRay.setOrigin(cam->getRealPosition()); camRay.setDirection(cam->getRealDirection()); }

int inner_form_loop(int windows_in_buf,dataptr dz) { int exit_status; int wc; double pre_amptotal, post_amptotal; for(wc=0; wc<windows_in_buf; wc++) { if(dz->total_windows==0) { if((exit_status = move_along_formant_buffer(dz))!=CONTINUE) return(exit_status); dz->flbufptr[0] += dz->wanted; dz->total_windows++; dz->time = (float)(dz->time + dz->frametime); continue; /* SKIP FIRST (ZERO-AMP) WINDOW */ } if(dz->mode==FORM_REPLACE) { rectify_window(dz->flbufptr[0],dz); if((exit_status = extract_specenv(0,0,dz))<0) return(exit_status); } dz->specenvamp2 = dz->flbufptr[1]; pre_amptotal = post_amptotal = 0.0; if((exit_status = get_totalamp(&pre_amptotal,dz->flbufptr[0],dz->wanted))<0) return(exit_status); if((exit_status = do_specform(dz))<0) return(exit_status); if((exit_status = form_filter(dz))<0) return(exit_status); if((exit_status = get_totalamp(&post_amptotal,dz->flbufptr[0],dz->wanted))<0) return(exit_status); if((exit_status = normalise(pre_amptotal,post_amptotal,dz))<0) return(exit_status); if(!flteq(dz->param[FORM_GAIN],1.0)) { if((exit_status = form_gain(dz))<0) return(exit_status); /* NB This check must be BEFORE move_along_formant_buffer() */ } dz->flbufptr[0] += dz->wanted; if(++dz->total_windows >= dz->wlength) return(FINISHED); if((exit_status = move_along_formant_buffer(dz))!=CONTINUE) return(exit_status); dz->time = (float)(dz->time + dz->frametime); } return(CONTINUE); }

/* This function simulates a number of scans around the object, then writes the scans to files along with all the grasps found */ void DBaseBatchPlanner::takeScans() { ScanSimulator sim; sim.setOptics(-45.0 , 45.0 , 400 , -45.0 , 45.0 , 400 ); sim.setType(ScanSimulator::WORLD_COORDINATES); int numAltitudes = 3; float altitudes[3] = {(float)M_PI/6.0f, (float)M_PI/3.0f, (float)M_PI/2.0f }; int numSamples[3] = {8, 4, 1}; float distance = 1000; //1 meter std::vector<position> cloud; std::vector<RawScanPoint> rawData; for (int i=0; i<numAltitudes; i++) { float theta = altitudes[i]; float phi_step = M_PI / numSamples[i]; for (int j=0; j<numSamples[i]; j++) { float phi = phi_step * j; vec3 loc( distance * cos(theta) * cos(phi), distance * cos(theta) * sin(phi), distance * sin(theta) ); //looking back towards the origin vec3 dir = -1 * normalise(loc); vec3 up(0,0,1); sim.setPosition(loc, dir, up, ScanSimulator::STEREO_CAMERA); cloud.clear(); rawData.clear(); sim.scan(&cloud, &rawData); writeCloudToFile(i, j, cloud); vec3 foo, trueUp; position foop; sim.getPosition(foop,foo,trueUp); writeRawToFile(i, j, rawData, loc, dir, trueUp); } } }

/* takes mouse position on screen and return ray in world coords */ vec3 get_ray_from_mouse (float mouse_x, float mouse_y) { // screen space (viewport coordinates) float x = (2.0f * mouse_x) / g_gl_width - 1.0f; float y = 1.0f - (2.0f * mouse_y) / g_gl_height; float z = 1.0f; // normalised device space vec3 ray_nds = vec3 (x, y, z); // clip space vec4 ray_clip = vec4 (ray_nds.v[0], ray_nds.v[1], -1.0, 1.0); // eye space vec4 ray_eye = inverse (proj_mat) * ray_clip; ray_eye = vec4 (ray_eye.v[0], ray_eye.v[1], -1.0, 0.0); // world space vec3 ray_wor = vec3 (inverse (view_mat) * ray_eye); // don't forget to normalise the vector at some point ray_wor = normalise (ray_wor); return ray_wor; }

void calcnormal(CTriangle3d t, CPoint3d &normal) { CPoint3d v1, v2; v1.x = t.aX - t.bX; v1.y = t.aY - t.bY; v1.z = t.aZ - t.bZ; v2.x = t.bX - t.cX; v2.y = t.bY - t.cY; v2.z = t.bZ - t.cZ; normal.x = v1.y * v2.z - v1.z * v2.y; normal.y = v1.z * v2.x - v1.x * v2.z; normal.z = v1.x * v2.y - v1.y * v2.x; normalise(normal); }

void Board::newellSquare(GLdouble *vec1,GLdouble *vec2,GLdouble *vec3,GLdouble *vec4,GLdouble *normal) { normal[0] = (vec1[1]-vec2[1])*(vec1[2]+vec2[2]) + (vec2[1]-vec3[1])*(vec2[2]+vec3[2]) + (vec3[1]-vec4[1])*(vec3[2]+vec4[2]) + (vec4[1]-vec1[1])*(vec4[2]+vec1[2]); normal[1] = (vec1[2]-vec2[2])*(vec1[0]+vec2[0]) + (vec2[2]-vec3[2])*(vec2[0]+vec3[0]) + (vec3[2]-vec4[2])*(vec3[0]+vec4[0]) + (vec4[2]-vec1[2])*(vec4[0]+vec1[0]); normal[2] = (vec1[0]-vec2[0])*(vec1[1]+vec2[1]) + (vec2[0]-vec3[0])*(vec2[1]+vec3[1]) + (vec3[0]-vec4[0])*(vec3[1]+vec4[1]) + (vec4[0]-vec1[0])*(vec4[1]+vec1[1]); normalise(normal); }

double PitchDetector::detectSdfPitchForBlock (float* samples, int numSamples) { const int minSample = int (sampleRate / maxFrequency); const int maxSample = int (sampleRate / minFrequency); lowFilter.reset(); highFilter.reset(); lowFilter.processSamples (samples, numSamples); highFilter.processSamples (samples, numSamples); sdfAutocorrelate (samples, numSamples, buffer1.getData()); normalise (buffer1.getData(), buffer1.getSize()); // find first minimum that is below a threshold const float threshold = 0.25f; const float* sdfData = buffer1.getData(); float min = 1.0f; int index = 0; for (int i = minSample; i < maxSample; ++i) { const float prevSample = sdfData[i - 1]; const float sample = sdfData[i]; const float nextSample = sdfData[i + 1]; if (sample < prevSample && sample < nextSample && sample < threshold) { if (sample < min) { min = sample; index = i; } // return sampleRate / i; // break; } } if (index != 0) return sampleRate / index; return 0.0; }

/*! \fn CvBinGabAdaFeatureSelect::train(int nfeatures) */ void CvBinGabAdaFeatureSelect::train(int nfeatures) { double error = 0.0; nselecfeatures = 0; nexpfeatures = nfeatures; char *signfilename = new char[30]; strcpy(signfilename, "significant.txt"); FILE * file = fopen(signfilename, "w"); fclose( file ); delete [] signfilename; for (int i = 0; i < nexpfeatures; i++) { current = i; if (old_pool->getSize() < 1) break; /* prepare log files */ char *allfeaturefilename = new char[30]; sprintf(allfeaturefilename, "allfeature_%d.txt", current); FILE * file1 = fopen(allfeaturefilename, "w"); fclose( file1 ); delete [] allfeaturefilename; char *disfeaturefilename = new char[30]; sprintf(disfeaturefilename, "discard_%d.txt", current); FILE * file2 = fopen(disfeaturefilename, "w"); fclose( file2 ); delete [] disfeaturefilename; char *featurefilename = new char[30]; sprintf(featurefilename, "feature_%d.txt", current); FILE * file3 = fopen(featurefilename, "w"); fclose( file3 ); delete [] featurefilename; /* prepare log files */ normalise(); trainallfeatures(); error = findminerror(); update(); saveweaks( weaksname.c_str() ); nselecfeatures++; } }

myVecteur2D::myVecteur2D(myVecteur2D* v1, myVecteur2D* v2){ // créer un rebond : premier vecteur sur le deuxième vertex originrebond; originrebond.x = 0; originrebond.y = 0; int sens; sens = intersectionDroites(v1, v2, originrebond); if (sens > 0) { v1->normalise(); v2->normalise(); //std::cout << "V1 : " << v1->getNorme() << " v2 : " << v2->getNorme() << " v1x : " << v1->getxdir() << " v1y : " << v1->getydir() << " v2x : " << v2->getxdir() << " v2y : " << v2->getydir() << std::endl; vertex dirOrtho; dirOrtho.x = v2->getydir(); dirOrtho.y = - v2->getxdir(); float psv12 = produitscalaire(v1,v2); //produit scalaire de v1 sur v2 float psv12ortho = v1->getxdir() * dirOrtho.x + v1->getydir() * dirOrtho.y; xdir = psv12*v2->getxdir() - psv12ortho * dirOrtho.x; ydir = psv12*v2->getydir() - psv12ortho * dirOrtho.y; origin = originrebond; //std::cout << "psv12 : " << psv12 << " ortho " << psv12ortho << "xdir : " << xdir << "direct " << psv12*v2->getxdir() + psv12ortho * dirOrtho.x<< std::endl; normalise(); } else { //on ne crée rien } }

void View::lookAt(double eyex, double eyey, double eyez, double centerx, double centery, double centerz, double twist) { Vec3 eye(eyex, eyey, eyez); Vec3 negz(0., 0., -1.); Vec3 dir; Quaternion q_dir, q_twist; center = Vec3(centerx, centery, centerz); dir = center-eye; zoom = len(dir); normalise(dir); q_dir = quatFromUV(negz, dir); q_twist = quatFromAxisRot(negz, twist * M_PI / 180); orient = conjugate(q_dir*q_twist); // quaternion multiplication glComputeModelview(); }

/*! Sets this quaternion by converting from rotation matrix \a R. */ void Quaternion::set(const mat3 &R) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". double trace = R.element(0,0)+R.element(1,1)+R.element(2,2); double root; if ( trace > 0.0 ) { // |w| > 1/2, may as well choose w > 1/2 root = sqrt(trace+1.0); // 2w w = 0.5*root; root = 0.5/root; // 1/(4w) x = (R.element(1,2)-R.element(2,1))*root; y = (R.element(2,0)-R.element(0,2))*root; z = (R.element(0,1)-R.element(1,0))*root; } else { // |w| <= 1/2 static int next[3] = { 1, 2, 0 }; int i = 0; if ( R.element(1,1) > R.element(0,0) ) i = 1; if ( R.element(2,2) > R.element(i,i) ) i = 2; int j = next[i]; int k = next[j]; root = sqrt(R.element(i,i)-R.element(j,j)-R.element(k,k)+1.0); double* quat[3] = { &x, &y, &z }; *quat[i] = 0.5*root; root = 0.5/root; w = (R.element(j,k)-R.element(k,j))*root; *quat[j] = (R.element(i,j)+R.element(j,i))*root; *quat[k] = (R.element(i,k)+R.element(k,i))*root; } normalise(); }

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

//build a quaternion from 3 euler angles Quaternion::Quaternion(double zdeg, double ydeg, double xdeg){ //if reverse order, change zdeg to xdeg and xdeg to zdeg //3-2-1 mQuatVal[0] = (cos(xdeg/2.0) * cos(ydeg/2.0) * cos(zdeg/2.0)) + (sin(xdeg/2.0) * sin(ydeg/2.0) * sin(zdeg/2.0)); mQuatVal[1] = (sin(xdeg/2.0) * cos(ydeg/2.0) * cos(zdeg/2.0)) - (cos(xdeg/2.0) * sin(ydeg/2.0) * sin(zdeg/2.0)); mQuatVal[2] = (cos(xdeg/2.0) * sin(ydeg/2.0) * cos(zdeg/2.0)) + (sin(xdeg/2.0) * cos(ydeg/2.0) * sin(zdeg/2.0)); mQuatVal[3] = (cos(xdeg/2.0) * cos(ydeg/2.0) * sin(zdeg/2.0)) - (sin(xdeg/2.0) * sin(ydeg/2.0) * cos(zdeg/2.0)); normalise(mQuatVal); mW = mQuatVal[0]; mVec = Vec3d(mQuatVal[1],mQuatVal[2],mQuatVal[3]); //angle always must be accurate - used for deriv mAngle = 2.0 * acos(mQuatVal[0]); double sinRes = sin(mAngle * .5); if(sinRes != 0 ) {//means theta is 0 or a multiple of 0 - only way theta/2 will have sin 0, so no rotation mAxis = mVec * (1.0 / sin(mAngle * .5)); } else {//if theta == 0 then no rotation, so doesn't matter mAxis = Vec3d(vl_0); mAxis[0] = 1; //arbitrary rotation - no rot needed if angle ==0 } }//