/** * transformRGB: applies current transformation to frame * Parameters: * td: private data structure of this filter * Return value: * 0 for failture, 1 for success * Preconditions: * The frame must be in RGB format /// TODO Add zoom! */ int _FLT(transformRGB)(TransformData* td, Transform t) { int x = 0, y = 0, z = 0; unsigned char *D_1, *D_2; D_1 = td->src; D_2 = td->destbuf; float c_s_x = td->fiSrc.width/2.0; float c_s_y = td->fiSrc.height/2.0; float c_d_x = td->fiDest.width/2.0; float c_d_y = td->fiDest.height/2.0; /* for each pixel in the destination image we calc the source * coordinate and make an interpolation: * p_d = c_d + M(p_s - c_s) + t * where p are the points, c the center coordinate, * _s source and _d destination, * t the translation, and M the rotation matrix * p_s = M^{-1}(p_d - c_d - t) + c_s */ /* All 3 channels */ if (fabs(t.alpha) > td->rotationThreshhold) { for (x = 0; x < td->fiDest.width; x++) { for (y = 0; y < td->fiDest.height; y++) { float x_d1 = (x - c_d_x); float y_d1 = (y - c_d_y); float x_s = cos(-t.alpha) * x_d1 + sin(-t.alpha) * y_d1 + c_s_x -t.x; float y_s = -sin(-t.alpha) * x_d1 + cos(-t.alpha) * y_d1 + c_s_y -t.y; for (z = 0; z < 3; z++) { // iterate over colors unsigned char* dest = &D_2[(x + y * td->fiDest.width)*3+z]; _FLT(interpolateN)(dest, x_s, y_s, D_1, td->fiSrc.width, td->fiSrc.height, 3, z, td->crop ? 16 : *dest); } } } } else { /* no rotation, just translation *(also no interpolation, since no size change (so far) */ int round_tx = myround(t.x); int round_ty = myround(t.y); for (x = 0; x < td->fiDest.width; x++) { for (y = 0; y < td->fiDest.height; y++) { for (z = 0; z < 3; z++) { // iterate over colors short p = PIXELN(D_1, x - round_tx, y - round_ty, td->fiSrc.width, td->fiSrc.height, 3, z, -1); if (p == -1) { if (td->crop == 1) D_2[(x + y * td->fiDest.width)*3+z] = 16; } else { D_2[(x + y * td->fiDest.width)*3+z] = (unsigned char)p; } } } } } return 1; }

GAIAGEO_DECLARE double gaiaGreatCircleDistance (double a, double b, double lat1, double lon1, double lat2, double lon2) { /* / Calculate great-circle distance (in m) between two points specified by / latitude/longitude (in decimal degrees) using Aviation Formulary / / http://williams.best.vwh.net/avform.htm#Dist / */ double latrad1 = lat1 * DEG2RAD; double lonrad1 = lon1 * DEG2RAD; double latrad2 = lat2 * DEG2RAD; double lonrad2 = lon2 * DEG2RAD; double avg_radius; double k1 = (sin ((latrad1 - latrad2) / 2.0)); double k2 = (sin ((lonrad1 - lonrad2) / 2.0)); double dist; dist = 2.0 * asin (sqrt (k1 * k1 + cos (latrad1) * cos (latrad2) * k2 * k2)); if (dist < 0.0) dist = dist + PI; if (a == b) avg_radius = a; else avg_radius = (2.0 * a + b) / 3.0; dist = dist * avg_radius; return dist; }

//! [4] void Edge::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *) { if (!source || !dest) return; QLineF line(sourcePoint, destPoint); if (qFuzzyCompare(line.length(), qreal(0.))) return; //! [4] //! [5] // Draw the line itself painter->setPen(QPen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); painter->drawLine(line); //! [5] //! [6] // Draw the arrows double angle = ::acos(line.dx() / line.length()); if (line.dy() >= 0) angle = TwoPi - angle; QPointF sourceArrowP1 = sourcePoint + QPointF(sin(angle + Pi / 3) * arrowSize, cos(angle + Pi / 3) * arrowSize); QPointF sourceArrowP2 = sourcePoint + QPointF(sin(angle + Pi - Pi / 3) * arrowSize, cos(angle + Pi - Pi / 3) * arrowSize); QPointF destArrowP1 = destPoint + QPointF(sin(angle - Pi / 3) * arrowSize, cos(angle - Pi / 3) * arrowSize); QPointF destArrowP2 = destPoint + QPointF(sin(angle - Pi + Pi / 3) * arrowSize, cos(angle - Pi + Pi / 3) * arrowSize); painter->setBrush(Qt::black); painter->drawPolygon(QPolygonF() << line.p1() << sourceArrowP1 << sourceArrowP2); painter->drawPolygon(QPolygonF() << line.p2() << destArrowP1 << destArrowP2); }

void SelfLocator::initSamplesAtGivenPositions(const vector<Pose2D>& poses, const vector< Pose2D >& standardDeviations) { for(int i = 0; i < samples->size(); ++i) { Sample& sample(samples->at(i)); if(poses.size()) { // Select one of the poses from the list: unsigned int index = random((short)poses.size()); Pose2D pose = poses[index]; Pose2D stdDev = standardDeviations[index]; // Create sample: sample.translation.x = static_cast<int>(pose.translation.x); sample.translation.x += sampleTriangularDistribution(static_cast<int>(stdDev.translation.x)); sample.translation.y = static_cast<int>(pose.translation.y); sample.translation.y += sampleTriangularDistribution(static_cast<int>(stdDev.translation.y)); float rotation = normalize(pose.rotation + sampleTriangularDistribution(stdDev.rotation)); sample.angle = rotation; sample.rotation = Vector2<int>(int(cos(rotation) * 1024), int(sin(rotation) * 1024)); } else //No given positions, spread uniformly: { Pose2D pose(bb->theFieldDimensions.randomPoseOnField()); sample.translation = Vector2<int>(int(pose.translation.x), int(pose.translation.y)); sample.angle = pose.rotation; sample.rotation = Vector2<int>(int(cos(pose.rotation) * 1024), int(sin(pose.rotation) * 1024)); } } lastOdometry = bb->theOdometryData; poseCalculator->init(); }

void CFPCamera::SetFPCamera(Vector3F & pos, float fPhi, float fTheta) { m_Pos = pos; m_fPhi = fPhi; m_fTheta = fTheta; m_RightVec.x = cosf(m_fPhi-D3DX_PI/2.); m_RightVec.y = 0; m_RightVec.z = sinf(m_fPhi-D3DX_PI/2.); m_ForwardVec.x = cos(m_fPhi)*cos(m_fTheta); m_ForwardVec.y = sin(m_fTheta); m_ForwardVec.z = sin(m_fPhi)*cos(m_fTheta); m_ForwardVec = m_ForwardVec.Normalize(); m_UpVec = CrossXYZ(m_ForwardVec, m_RightVec); float x = -DotXYZ(m_Pos, m_RightVec); float y = -DotXYZ(m_Pos, m_UpVec); float z = -DotXYZ(m_Pos, m_ForwardVec); m_View.m[0][0] = m_RightVec.x, m_View.m[1][0] = m_RightVec.y, m_View.m[2][0] = m_RightVec.z, m_View.m[3][0] = x; m_View.m[0][1] = m_UpVec.x, m_View.m[1][1] = m_UpVec.y, m_View.m[2][1] = m_UpVec.z, m_View.m[3][1] = y; m_View.m[0][2] = m_ForwardVec.x, m_View.m[1][2] = m_ForwardVec.y, m_View.m[2][2] = m_ForwardVec.z, m_View.m[3][2] = z; m_View.m[0][3] = 0, m_View.m[1][3] = 0, m_View.m[2][3] = 0, m_View.m[3][3] = 1; }

unsigned int set_plaspix(float x, float y, t_plsm *data) { return (((63.0 + (63.0 * sin(x / 16.0)) + 63.0 + (63.0 * sin(y / data->zoom)) + 63.0 + (63 * sin((x + y + data->move) / data->modif)) + 63.0 + (63 * sin(sqrt(x * x + y * y) / 2))) / 7)); }

static double psc_harris_init_field(struct psc *psc, double x[3], int m) { struct psc_harris *harris = to_psc_harris(psc); double BB = harris->B0; double LLz = psc->domain.length[2], LLy = psc->domain.length[1]; double LLL = harris->LLL; double AA = harris->AA; switch (m) { case HZ: return BB * tanh((x[1]) / LLL) - AA * M_PI/LLy * cos(2.*M_PI * (x[2] - .5 * LLz) / LLz) * sin(M_PI * x[1] / LLy); case HY: return AA * 2.*M_PI / LLz * sin(2.*M_PI * (x[2] - .5 * LLz) / LLz) * cos(M_PI * x[1] / LLy); case JXI: return BB / LLL * (1./sqr(cosh(x[1] / LLL))) - AA * sqr(M_PI) * (1./sqr(LLy) + 4./sqr(LLz)) * cos(2.*M_PI * (x[2] - .5 *LLz) / LLz) * cos(M_PI * x[1] / LLy); default: return 0.; } }

// Retourne : une liste de points représentant un arc de cercle // A utiliser dans le dessin des boutons ei_linked_point_t* ei_draw_arc(ei_point_t centre, float R, float angledebut, float anglefin) { ei_linked_point_t* liste = NULL; float angle = angledebut; while (angle < anglefin) { ei_linked_point_t* new_point = malloc(sizeof(ei_linked_point_t)); new_point->point = ei_point( (int)(R*cos(angle)) + centre.x, (int)(R*sin(angle)) + centre.y); new_point->next = NULL; concat_lists(&liste, &new_point); // Pas de rotation = 0.1 angle = angle + 0.1; } ei_linked_point_t* last_point = malloc(sizeof(ei_linked_point_t)); last_point->point = ei_point( (int)(R*cos(anglefin)) + centre.x, (int)(R*sin(anglefin)) + centre.y); last_point->next = NULL; concat_lists(&liste, &last_point); return liste; }

// main void main(void) { vec2 uv = gl_FragCoord.xy / iResolution.xy; uv = uv * 2.0 - 1.0; uv.x *= iResolution.x / iResolution.y; float time = iGlobalTime * 0.3; // ray vec3 ang = vec3(sin(time*3.0)*0.1,sin(time)*0.2+0.4,time); if(iMouse.z > 0.0) ang = vec3(0.0,clamp(2.0-iMouse.y*0.01,-0.3,PI),iMouse.x*0.01); mat4 rot = fromEuler(ang); vec3 ori = vec3(0.0,0.2,time*1.0); ori.y += abs(map_detailed(-ori)); vec3 dir = normalize(vec3(uv.xy,-1.0)); dir = rotate(normalize(dir),rot); // tracing vec3 p; float dens = hftracing(ori,dir,p); vec3 dist = p - ori; vec3 n = getNormal(p, dot(dist,dist)*EPSILON_NRM); // color vec3 color = sea_color(p,n,dir,dist); vec3 light = normalize(vec3(0.0,1.0,0.8)); color += vec3(diffuse(n,light,80.0) * SEA_WATER_COLOR) * 0.12; color += vec3(specular(n,light,dir,60.0)); // post color = mix(sky_color(dir),color, clamp(1.0-length(dist)/100.0,0.0,1.0)); color = pow(color,vec3(0.75)); gl_FragColor = vec4(color,1.0); }

void Camera::updateCameraRotation() { double xPos, yPos; glfwGetCursorPos(m_window, &xPos, &yPos); // get current mouse position // if mouse still in window center, no need to recalculate view matrix if (xPos == m_windowWidth / 2 && yPos == m_windowHeight / 2) { return; } centerMouse(); m_horizontalAngle += float(m_windowWidth / 2 - xPos) * m_mouseSpeed; m_verticalAngle += float(m_windowHeight / 2 - yPos) * m_mouseSpeed; glm::vec3 oldLookIn = getLookAt() - getCenter(); glm::vec3 lookInHorizontal = glm::vec3(sin(m_horizontalAngle), oldLookIn.y, cos(m_horizontalAngle)); glm::vec3 lookInVertical = glm::vec3(oldLookIn.x * cos(m_verticalAngle), sin(m_verticalAngle), oldLookIn.z * cos(m_verticalAngle)); glm::vec3 newLookIn = glm::normalize(lookInHorizontal + lookInVertical); setLookAt(getCenter() + newLookIn); float rightHorizontalAngle = m_horizontalAngle - PI / 2.0f; setRight(glm::vec3(sin(rightHorizontalAngle), getRight().y, cos(rightHorizontalAngle))); setUp(glm::normalize(glm::cross(getRight(), newLookIn))); }

/******************************************************************************* * Function Name : ComputeViewMatrix * Description : Calculate the view matrix turning around the balloon *******************************************************************************/ void OGLES3Skybox2::ComputeViewMatrix() { PVRTVec3 vFrom; /* Calculate the distance to balloon */ float fDistance = fViewDistance + fViewAmplitude * (float) sin(fViewAmplitudeAngle); fDistance = fDistance / 5.0f; fViewAmplitudeAngle += 0.004f; /* Calculate the vertical position of the camera */ float updown = fViewUpDownAmplitude * (float) sin(fViewUpDownAngle); updown = updown / 5.0f; fViewUpDownAngle += 0.005f; /* Calculate the angle of the camera around the balloon */ vFrom.x = fDistance * (float) cos(fViewAngle); vFrom.y = updown; vFrom.z = fDistance * (float) sin(fViewAngle); fViewAngle += 0.003f; /* Compute and set the matrix */ m_mView = PVRTMat4::LookAtRH(vFrom, vTo, vUp); /* Remember the camera position to draw the Skybox around it */ vCameraPosition = vFrom; }

void Camera::mouseDrag(double x, double y) { static bool firstMouse = true; if (firstMouse) { lastX = (float)x; lastY = (float)y; firstMouse = false; } Float xOffset = (float)x - lastX; Float yOffset = (float)y - lastY; lastX = (float)x; lastY = (float)y; Float sensitivity = 0.05f; xOffset *= sensitivity; yOffset *= sensitivity; yaw += xOffset; pitch += yOffset; if (pitch > 89.0f) pitch = 89.0f; if (pitch < -89.0f) pitch = -89.0f; front = glm::normalize(Vector( cos(glm::radians(yaw)) * cos(glm::radians(pitch)), sin(glm::radians(pitch)), sin(glm::radians(yaw)) * cos(glm::radians(pitch)) )); }

void GICPOptimizer::compute_dr(gsl_vector const* x, gsl_matrix const* gsl_temp_mat_r, gsl_vector *g) { double dR_dPhi[3][3]; double dR_dTheta[3][3]; double dR_dPsi[3][3]; gsl_matrix_view gsl_d_rx = gsl_matrix_view_array(&dR_dPhi[0][0],3, 3); gsl_matrix_view gsl_d_ry = gsl_matrix_view_array(&dR_dTheta[0][0],3, 3); gsl_matrix_view gsl_d_rz = gsl_matrix_view_array(&dR_dPsi[0][0],3, 3); double phi = gsl_vector_get(x ,3); double theta = gsl_vector_get(x ,4); double psi = gsl_vector_get(x ,5); double cphi = cos(phi), sphi = sin(phi); double ctheta = cos(theta), stheta = sin(theta); double cpsi = cos(psi), spsi = sin(psi); dR_dPhi[0][0] = 0.; dR_dPhi[1][0] = 0.; dR_dPhi[2][0] = 0.; dR_dPhi[0][1] = sphi*spsi + cphi*cpsi*stheta; dR_dPhi[1][1] = -cpsi*sphi + cphi*spsi*stheta; dR_dPhi[2][1] = cphi*ctheta; dR_dPhi[0][2] = cphi*spsi - cpsi*sphi*stheta; dR_dPhi[1][2] = -cphi*cpsi - sphi*spsi*stheta; dR_dPhi[2][2] = -ctheta*sphi; dR_dTheta[0][0] = -cpsi*stheta; dR_dTheta[1][0] = -spsi*stheta; dR_dTheta[2][0] = -ctheta; dR_dTheta[0][1] = cpsi*ctheta*sphi; dR_dTheta[1][1] = ctheta*sphi*spsi; dR_dTheta[2][1] = -sphi*stheta; dR_dTheta[0][2] = cphi*cpsi*ctheta; dR_dTheta[1][2] = cphi*ctheta*spsi; dR_dTheta[2][2] = -cphi*stheta; dR_dPsi[0][0] = -ctheta*spsi; dR_dPsi[1][0] = cpsi*ctheta; dR_dPsi[2][0] = 0.; dR_dPsi[0][1] = -cphi*cpsi - sphi*spsi*stheta; dR_dPsi[1][1] = -cphi*spsi + cpsi*sphi*stheta; dR_dPsi[2][1] = 0.; dR_dPsi[0][2] = cpsi*sphi - cphi*spsi*stheta; dR_dPsi[1][2] = sphi*spsi + cphi*cpsi*stheta; dR_dPsi[2][2] = 0.; // set d/d_rx = tr(dR_dPhi'*gsl_temp_mat_r) [= <dR_dPhi, gsl_temp_mat_r>] gsl_vector_set(g, 3, mat_inner_prod(&gsl_d_rx.matrix, gsl_temp_mat_r)); // set d/d_ry = tr(dR_dTheta'*gsl_temp_mat_r) = [<dR_dTheta, gsl_temp_mat_r>] gsl_vector_set(g, 4, mat_inner_prod(&gsl_d_ry.matrix, gsl_temp_mat_r)); // set d/d_rz = tr(dR_dPsi'*gsl_temp_mat_r) = [<dR_dPsi, gsl_temp_mat_r>] gsl_vector_set(g, 5, mat_inner_prod(&gsl_d_rz.matrix, gsl_temp_mat_r)); }

std::vector<geometry_msgs::Pose> RoadNavigation::getTargets(geometry_msgs::Pose current_pose, nav_msgs::Path target_traj) { double min_distance = 5.; // In meters, along the traj int index = 0; for (; getDistance(target_traj.poses[0].pose, target_traj.poses[index].pose) < min_distance; index++); index--; geometry_msgs::Pose end; end.position.x = target_traj.poses[index].pose.position.x; end.position.y = target_traj.poses[index].pose.position.y; end.position.z = target_traj.poses[index].pose.position.z; double m = atan((current_pose.position.y - end.position.y) / (current_pose.position.x - end.position.x)); double dist_seeds = 10; // Need to get laterally shifted trajectories std::vector<geometry_msgs::Pose> targets; geometry_msgs::Pose poss_target; for (int i = 1; i < 40; i += 1) { poss_target.position.x = end.position.x + i * dist_seeds * sin(m); poss_target.position.y = end.position.y - i * dist_seeds * cos(m); poss_target.position.z = end.position.z; targets.push_back(poss_target); } for (int i = -1; i>-40; i -= 1) { poss_target.position.x = end.position.x + i * dist_seeds * sin(m); poss_target.position.y = end.position.y - i * dist_seeds * cos(m); poss_target.position.z = end.position.z; targets.push_back(poss_target); } return targets; }

inline float solution_biharmonic(float X,float Y,float Z){ float pi=M_PI; float analytic=0; float SIGMA2=SIGMA*SIGMA; float SIGMA3=SIGMA*SIGMA2; float SIGMA4=SIGMA*SIGMA3; float f=std::exp( -SIGMA * ( (X-pi)*(X-pi) + (Y-pi)*(Y-pi) + (Z-pi)*(Z-pi) )); float Xp=X-pi; float Yp=Y-pi; float Zp=Z-pi; //float fxx=-2*SIGMA*f+4*SIGMA2*Xp*Xp*f; //float fyy=-2*SIGMA*f+4*SIGMA2*Yp*Yp*f; //float fzz=-2*SIGMA*f+4*SIGMA2*Zp*Zp*f; float fxxxx=12*SIGMA2*f-48*SIGMA3*Xp*Xp*f+16*SIGMA4*Xp*Xp*Xp*Xp*f; float fyyyy=12*SIGMA2*f-48*SIGMA3*Yp*Yp*f+16*SIGMA4*Yp*Yp*Yp*Yp*f; float fzzzz=12*SIGMA2*f-48*SIGMA3*Zp*Zp*f+16*SIGMA4*Zp*Zp*Zp*Zp*f; float fxxyy=4*SIGMA2*f-8*SIGMA3*Yp*Yp*f-8*SIGMA3*Xp*Xp*f+16*SIGMA4*Xp*Xp*Yp*Yp*f; float fxxzz=4*SIGMA2*f-8*SIGMA3*Zp*Zp*f-8*SIGMA3*Xp*Xp*f+16*SIGMA4*Xp*Xp*Zp*Zp*f; float fyyzz=4*SIGMA2*f-8*SIGMA3*Zp*Zp*f-8*SIGMA3*Yp*Yp*f+16*SIGMA4*Yp*Yp*Zp*Zp*f; analytic+=C0*9*sin(X)*sin(Y)*sin(Z)+C1*(fxxxx+2*fxxyy+2*fxxzz+fyyyy+2*fyyzz+fzzzz); if(analytic!=analytic) analytic=0; return analytic; } // end solution_biharmonic

Bala::Bala(float _v, sf::Sprite& _cannon, sf::Image& imagen) { float cannonA, cannonX, cannonY, cannonH; //Cargo sprite sprite.SetImage(imagen); sprite.SetCenter(imagen.GetWidth()/2, imagen.GetHeight()/2); //Inicializo variables dt = 0.0f; cannonA = _cannon.GetRotation(); cannonX = _cannon.GetPosition().x; cannonY = _cannon.GetPosition().y; //Tomo el tamaño sobre el eje x como el largo del cañon //porque sin rotar la misma se encuentra en horizontal cannonH = _cannon.GetSize().x; //Descompongo la velocidad inicial vx = _v * (cos(cannonA/180*PI)); vy = _v * (sin(cannonA/180*PI)); //Posicion inicial bala según la posición del cañon //usando trigonometria xi = cannonX + ((cos(cannonA/180*PI) * cannonH)); yi = cannonY - ((sin(cannonA/180*PI) * cannonH)); sprite.SetPosition(xi, yi); }

unsigned int CContactTarget3D_Mesh::FindInOut_Boxel (double px, double py, double pz) const { assert( pBoxel_ != 0 ); double p[3] = { px, py, pz }; if( pBoxel_->IsOut(p) && !is_hole ){ return 1; } unsigned int icnt_in = 0; unsigned int icnt_out = 0; for(unsigned int i=0;i<10;i++){ const double theta = i*6.28/10+0.15432452356673; const double dir[3] = { sin(theta)*cos(theta*2), sin(theta)*sin(theta*2), cos(theta) }; // const double dir[3] = { -0.35, 0.1342, 0.3 }; unsigned int ires = FindInOut_IntersectionRay_Boxel(px,py,pz, dir); // unsigned int ires1 = FindInOut_IntersectionRay(px,py,pz, dir); /* if( ires != ires1 ){ std::cout << "hoge " << px << " " << py << " " << pz << std::endl; }*/ if( ires != 2 && !is_hole ){ return ires; } if( ires == 0 ) icnt_in++; if( ires == 1 ) icnt_out++; } if( icnt_in > 5 ) return 0; if( icnt_out > 5 ) return 1; return 2; }

//-------------------------------------------------------------- void testApp::updateLines() { // Set the color back to default. dancerList[ 0 ].c.set( ofColor( 255, 0, 0 ) ); dancerList[ 1 ].c.set( ofColor( 0, 255, 0 ) ); dancerList[ 2 ].c.set( ofColor( 0, 0, 255 ) ); // Change the color upon collision. This isn't working right now and it also makes the app run really slowly, so I'm turning it off. /*for ( int i = 0; i < dancerList.size(); i++ ) { for ( int j = 0; j < dancerList.size(); j++ ) { for ( int k = 0; k < dancerList[ i ].lineList.size(); k++ ) { for ( int l = 0; l < dancerList[ j ].lineList.size(); l++ ) { if ( dancerList[ i ].lineList[ k ].pos == dancerList[ j ].lineList[ l ].pos ) { dancerList[ i ].lineList[ k ].c = ofColor(255); dancerList[ j ].lineList[ l ].c = ofColor(255); } } } } }*/ // When we have real breathing data we won't need this, but for the time being, let's fake it. float waveSpeed = 1; if ( !noiseBreath ) { breath = sin( ofGetElapsedTimef() * waveSpeed ) * breathRad; } else { breath = ofNoise( sin( ofGetElapsedTimef() * waveSpeed ) ) * breathRad; } // Pass in the data and update the Dancers. for ( int i = 0; i < dancerList.size(); i++ ) { dancerList[ i ].update( breath ); } }

/** * Vector a is rotated along each axis by the number of degrees specified by that component in the rotation vector. * This means that if you pass the rotation vector (45, 90, 0), vector a will be rotated 45 degrees along the * i axis, 90 degrees along the j axis, and 0 degrees along the k axis. */ DLLEX Vector *vectorRotate(Vector *a, Vector *rotation) { Vector *rotated = vectorCopy(a); double *i = &(rotated->components[0]), // declare pointers to save typing, and avoid having to reassign each component by hand *j = &(rotated->components[1]), // even if it means more memory references because I'm lazy. *k = &(rotated->components[2]); double iRot = rotation->components[0] * PI / 180.0, // Convert each rotational value to radians jRot = rotation->components[1] * PI / 180.0, kRot = rotation->components[2] * PI / 180.0; double sinI = sin(iRot), // Compute the sine and cosine of each rotation to avoid computing them multiple times cosI = cos(iRot), sinJ = sin(jRot), cosJ = cos(jRot), sinK = sin(kRot), cosK = cos(kRot); // rotate around i axis (i component remains constant) // not hardcore enough to write the rotations as one-liners *j = (*j)*cosI - (*k)*sinI; *k = (*j)*sinI + (*k)*cosI; // rotate around j axis (j component remains constant) *i = (*k)*sinJ + (*i)*cosJ; *k = (*k)*cosJ - (*i)*sinJ; // rotate around k axis (k component remains constant) *i = (*i)*cosK - (*j)*sinK; *j = (*i)*sinK + (*j)*cosK; return rotated; }

FGLocation::FGLocation(double lon, double lat, double radius) : mCacheValid(false) { e2 = c = 0.0; a = ec = ec2 = 1.0; epa = 0.0; mLon = mLat = mRadius = 0.0; mGeodLat = GeodeticAltitude = 0.0; mTl2ec.InitMatrix(); mTec2l.InitMatrix(); mTi2ec.InitMatrix(); mTec2i.InitMatrix(); mTi2l.InitMatrix(); mTl2i.InitMatrix(); double sinLat = sin(lat); double cosLat = cos(lat); double sinLon = sin(lon); double cosLon = cos(lon); mECLoc = FGColumnVector3( radius*cosLat*cosLon, radius*cosLat*sinLon, radius*sinLat ); }

void cosft1(float y[], int n) { void realft(float data[], unsigned long n, int isign); int j,n2; float sum,y1,y2; double theta,wi=0.0,wpi,wpr,wr=1.0,wtemp; theta=PI/n; wtemp=sin(0.5*theta); wpr = -2.0*wtemp*wtemp; wpi=sin(theta); sum=0.5*(y[1]-y[n+1]); y[1]=0.5*(y[1]+y[n+1]); n2=n+2; for (j=2;j<=(n>>1);j++) { wr=(wtemp=wr)*wpr-wi*wpi+wr; wi=wi*wpr+wtemp*wpi+wi; y1=0.5*(y[j]+y[n2-j]); y2=(y[j]-y[n2-j]); y[j]=y1-wi*y2; y[n2-j]=y1+wi*y2; sum += wr*y2; } realft(y,n,1); y[n+1]=y[2]; y[2]=sum; for (j=4;j<=n;j+=2) { sum += y[j]; y[j]=sum; } }

// Public general function int JuliaSet::Iterate( const int p_X, const int p_Y ) { ComplexNumber<double> c; c.x = (sin(cos(m_Zoom / 10) * 10) + cos(m_Zoom * 2.0) / 4.0 + sin(m_Zoom * 3.0) / 6.0) * 0.8; c.y = (cos(sin(m_Zoom / 10) * 10) + sin(m_Zoom * 2.0) / 4.0 + cos(m_Zoom * 3.0) / 6.0) * 0.8; const double scalex = 3.0; const double scaley = 2.0; const double diffx = -1.5f; const double diffy = -1.0f; ComplexNumber<double> z; z.x = ( scalex / ( (double)m_Width / (double)p_X ) + diffx ) / m_Zoom; z.y = ( scaley / ( (double)m_Height / (double)p_Y ) + diffy) / m_Zoom; int n; for( n = 0; n < m_Precision; n++ ) { double newx = ( z.x * z.x - z.y * z.y ) + c.x; double newy = ( z.y * z.x + z.x * z.y ) + c.y; if( ( newx * newx + newy * newy ) > 4.0f ) { break; } z.x = newx; z.y = newy; } return n; }

static void gwy_data_field_mark_facets(GwyDataField *dtheta, GwyDataField *dphi, gdouble theta0, gdouble phi0, gdouble tolerance, GwyDataField *mask) { gdouble cr, cth0, sth0, cro; const gdouble *xd, *yd; gdouble *md; gint i; cr = cos(tolerance); cth0 = cos(theta0); sth0 = sin(theta0); xd = gwy_data_field_get_data_const(dtheta); yd = gwy_data_field_get_data_const(dphi); md = gwy_data_field_get_data(mask); for (i = gwy_data_field_get_xres(dtheta)*gwy_data_field_get_yres(dtheta); i; i--, xd++, yd++, md++) { cro = cth0*cos(*xd) + sth0*sin(*xd)*cos(*yd - phi0); *md = (cro >= cr); } }

//Private void UIElement::UpdateModel(){ bUpdateMe = false; glm::mat4 trans(1), rot(1), scale(1); trans[3] = v4Translate; if (m_parent == nullptr){ trans[3].z = App::GetDepth() * -0.5f; } trans[3].z += fOffset; scale[0][0] = v2Scale.x; scale[1][1] = v2Scale.y; rot[0][0] = cos(fRotation);rot[0][1] = -sin(fRotation); rot[1][0] = sin(fRotation);rot[1][1] = cos(fRotation); if (m_parent == nullptr) { m4Model = trans * rot * scale; } else { m4Model = m_parent->m4Model * (trans * rot * scale); } for (auto child : mcuiElements){ if (child.second != nullptr) child.second->UpdateModel(); } }

void clParticleEmitter_Sphere::EmitParticles( const clPtr<clParticleSystem>& PS, float DeltaTime ) const { FAccumulatedTime += DeltaTime; while ( FAccumulatedTime > 1.0f / FEmissionRate && PS->GetParticles().size() < FMaxParticles ) { FAccumulatedTime -= 1.0f / FEmissionRate; sParticle P; float Theta = Math::RandomInRange( 0.0f, Math::TWOPI ); float Phi = Math::RandomInRange( 0.0f, Math::TWOPI ); float R = FRadius; float SinTheta = sin( Theta ); float x = R * SinTheta * cos( Phi ); float y = R * SinTheta * sin( Phi ); float z = R * cos( Theta ); P.FPosition = FCenter + vec3( x, y, z ); P.FVelocity = vec3( x, y, z ).GetNormalized() * Math::RandomInRange( FRadialVelocityMin, FRadialVelocityMax ); P.FAcceleration = vec3( 0.0f ); P.FTTL = Math::RandomInRange( FLifetimeMin, FLifetimeMax ); P.FLifeTime = P.FTTL; P.FRGBA = Math::RandomVector4InRange( FColorMin, FColorMax ); P.FRGBA.w = 1.0f; P.FSize = Math::RandomInRange( FSizeMin, FSizeMax ); PS->AddParticle( P ); } }

/* ================= R_SetupFrustum Setup that culling frustum planes for the current view ================= */ void R_SetupFrustum (void) { int i; float xs, xc; float ang; ang = tr.viewParms.fovX / 180 * M_PI * 0.5f; xs = sin( ang ); xc = cos( ang ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[0].normal ); VectorMA( tr.viewParms.frustum[0].normal, xc, tr.viewParms.ori.axis[1], tr.viewParms.frustum[0].normal ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[1].normal ); VectorMA( tr.viewParms.frustum[1].normal, -xc, tr.viewParms.ori.axis[1], tr.viewParms.frustum[1].normal ); ang = tr.viewParms.fovY / 180 * M_PI * 0.5f; xs = sin( ang ); xc = cos( ang ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[2].normal ); VectorMA( tr.viewParms.frustum[2].normal, xc, tr.viewParms.ori.axis[2], tr.viewParms.frustum[2].normal ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[3].normal ); VectorMA( tr.viewParms.frustum[3].normal, -xc, tr.viewParms.ori.axis[2], tr.viewParms.frustum[3].normal ); for (i=0 ; i<4 ; i++) { tr.viewParms.frustum[i].type = PLANE_NON_AXIAL; tr.viewParms.frustum[i].dist = DotProduct (tr.viewParms.ori.origin, tr.viewParms.frustum[i].normal); SetPlaneSignbits( &tr.viewParms.frustum[i] ); } }

float_type root_raised_cosine_imp(float_type alpha, float_type xin, float_type rate, long num_taps) //----------------------------------------------------------------------- // Calculates the square root raised cosine pulse shape given the // excess bandwidth value beta and the index. //----------------------------------------------------------------------- { float_type x1,x2,x3; float_type nom,denom; float_type xindx = xin-num_taps/2; x1 = PI*xindx/rate; x2 = 4*alpha*xindx/rate; x3 = x2*x2-1; if (x3!=0) { if (x1!=0) { nom = cos((1+alpha)*x1); nom += sin((1-alpha)*x1)/(4*alpha*xindx/rate); } else { nom = cos((1+alpha)*x1); nom += (1-alpha)*PI/(4*alpha); } denom = x3*PI; } else { if (alpha==1) return(-1); x3 = (1-alpha)*x1; x2 = (1+alpha)*x1; nom = sin(x2)*(1+alpha)*PI - cos(x3)*((1-alpha)*PI*rate)/(4*alpha*xindx) + sin(x3)*rate*rate/(4*alpha*xindx*xindx); denom = -32*PI*alpha*alpha*xindx/rate; } return(4*alpha*nom/denom); }

static void quaternion_from_axis_angle(struct quaternion *q, float ax, float ay, float az, float angle) { q->x = ax * sin(angle/2); q->y = ay * sin(angle/2); q->z = az * sin(angle/2); q->w = cos(angle/2); }

void Agent::Wander(float radius, float jitter, float distance, float weight) { auto &m = m_movementInfo; // determine some random angles for circle/jitter float cangle = rand(); float jangle = rand(); // create circle/jitter vectors float xCircle = cos(cangle) * radius; float yCircle = sin(cangle) * radius; float xJitter = cos(jangle) * jitter; float yJitter = sin(jangle) * jitter; //combine float xOffset = xCircle + xJitter; float yOffset = yCircle + yJitter; float offMag = sqrt(xOffset*xOffset + yOffset*yOffset); //normalize to size of circle xOffset *= radius / offMag; yOffset *= radius / offMag; // find normal of vector between, Mult by distance float velMag = sqrtf(m.m_velocityX*m.m_velocityX + m.m_velocityY*m.m_velocityY); float xPosition = (m.m_velocityX / velMag) * distance; float yPosition = (m.m_velocityY / velMag) * distance; // add our offset and circle position to the player's position to get our target float xTarget = xOffset + xPosition + m.m_positionX; float yTarget = yOffset + yPosition + m.m_positionY; Patrol(); }

Eigen::Matrix<T,4,1> blend(T t, Eigen::Matrix<T,4,1> p00, Eigen::Matrix<T,4,1> pNorm, Eigen::Matrix<T,4,1> p10){ T r0 = p00.mag(); T r1 = p10.mag(); T rs = linear(t,r0,r1); T dotp = dot(p00,p10); dotp = abs(dotp); T theta = acos(dotp/r0/r1); Eigen::Quaternion<T> dq0; p00.normalize(); p10.normalize(); Eigen::Quaternion<T> q00(p00); Eigen::Quaternion<T> q10(p10); if (theta < 0.01) { //just give it a little nudge in the right direction T o = 0.10; Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*pNorm[0], sin(o/2.)*pNorm[1], sin(o/2.)*pNorm[2]); Eigen::Matrix<T,4,1> p01 = dq.rotate(p00); Eigen::Quaternion<T> q01(p01); dq0 = Eigen::Quaternion<T>::slerp(q01,q10,t); } else { dq0 = Eigen::Quaternion<T>::slerp(q00,q10,t); } T dott = p00.dot(p10); T dotq0 = q00.dot(q10); T dotq1 = dq0.dot(q10); return Eigen::Matrix<T,4,1>(rs*dq0[1],rs*dq0[2],rs*dq0[3]); //return Eigen::Matrix<T,4,1>(p01); }