void meCloseHoles::ComputePatchForHole(vtkPolyDataNormals* normals, vtkPoints* points, vtkCellArray* polys) { vtkSmartPointer<vtkPointLocator> locator = vtkSmartPointer<vtkPointLocator>::New(); locator->SetDataSet(this->input); locator->BuildLocator(); int numEdgesInRegion = 0, nUsedIds, pointId, pointId1, pointId2; vtkSmartPointer<vtkCleanPolyData> clean = vtkSmartPointer<vtkCleanPolyData>::New(); vtkIdList* ptIds = vtkIdList::New(), * ptIds1 = vtkIdList::New(), * ptIds2 = vtkIdList::New(), * ptIdsOrdered = vtkIdList::New(), * cellIds = vtkIdList::New(), * cellIdsOrdered = vtkIdList::New(); vector<int> nptsInCorona, nptsUntilCorona; vector< vector<bool> > onlyOneCorrespondingPoint; vector< vector<double> > normalsAtPoint; ////For each hole (region) the points are stored and sorted //for(int idRegion = 0; idRegion < numRegions; idRegion++ ) //{ numEdgesInRegion = inputHole->GetNumberOfLines(); clean->SetInput(inputHole); clean->Update(); //For each hole, point and connectivity lists (not sorted) are created. int npts = clean->GetOutput()->GetNumberOfPoints(); clean->GetOutput()->BuildLinks(); //in order to sort clean->GetOutput()->BuildCells(); clean->GetOutput()->Update(); double center[3]={0,0,0}, point[3], point1[3], point2[3]; //points = vtkPoints::New(); ptIdsOrdered->Reset(); cellIdsOrdered->Reset(); int idUsed = 0; for(int id=0; id< numEdgesInRegion; id++) { cellIdsOrdered->InsertNextId(idUsed); clean->GetOutput()->GetCellPoints(idUsed,ptIds); ptIdsOrdered->InsertNextId(ptIds->GetId(0)); clean->GetOutput()->GetPointCells(ptIds->GetId(1),cellIds); idUsed = (cellIds->GetId(0) == idUsed ? cellIds->GetId(1) : idUsed=cellIds->GetId(0)); clean->GetOutput()->GetPoint(ptIds->GetId(0),point); points->InsertNextPoint(point); } for(int id=0; id<npts; id++) { clean->GetOutput()->GetPoint(id,point); for(int i=0; i<3; i++) center[i]+=point[i]; } for(int i=0; i<3; i++) center[i]/=npts; double normal[3]={0,0,0}; double area=0, theta, R, a, l, b, mn; if(this->closeHoleAlgorithm == SMOOTH_TO_CENTER || this->closeHoleAlgorithm == SMOOTH_PATCH) { points->GetPoint(npts-1,point1); for(int i=0; i<3; i++) point1[i]-=center[i]; for(int id=0; id< npts; id++) { points->GetPoint(id,point); for(int i=0; i<3; i++) { point[i]-=center[i]; point2[i]=point[i]-point1[i]; } for(int i=0; i<3; i++) normal[i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3]; for(int i=0; i<3; i++) point1[i]=point2[i]; } for(int i=0; i<3; i++) area+=pow(normal[i],2); area=sqrt(area); for(int i=0; i<3; i++) normal[i]/=area; } int numberOfCoronas=( this->closeHoleAlgorithm==SINGLE_CENTER_POINT ? 1 : npts/6+1 ); if(this->closeHoleAlgorithm == SMOOTH_PATCH) numberOfCoronas*=5; nptsInCorona.clear(); nptsInCorona.resize(numberOfCoronas); nptsUntilCorona.clear(); nptsUntilCorona.resize(numberOfCoronas+1); nptsUntilCorona[0]=0; nptsInCorona[0]=npts; onlyOneCorrespondingPoint.clear(); onlyOneCorrespondingPoint.resize(numberOfCoronas); onlyOneCorrespondingPoint[0].resize(npts); if(this->closeHoleAlgorithm == SMOOTH_PATCH) { normalsAtPoint.resize(npts); for(int id=0; id<npts; id++) { normalsAtPoint[id].clear(); normalsAtPoint[id].resize(3); points->GetPoint(id,point); pointId=locator->FindClosestPoint(point); normals->GetOutput()->GetPointCells(pointId,cellIds); // for(int i=0; i<3; i++) normalsAtPoint[id][i]=0; a=cellIds->GetNumberOfIds(); for(int j=0; j<a; j++) { normals->GetOutput()->GetCellPoints(cellIds->GetId(j),ptIds); int sameId; if(ptIds->GetId(0)==pointId) sameId=0; else if(ptIds->GetId(1)==pointId) sameId=1; else sameId=2; this->input->GetPoint(ptIds->GetId((sameId+1)%3),point1); this->input->GetPoint(ptIds->GetId((sameId+2)%3),point2); for(int i=0; i<3; i++) { point1[i]-=point[i]; point2[i]-=point[i]; } for(int i=0; i<3; i++) { normalsAtPoint[id][i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3]; } area=0; for(int i=0; i<3; i++) area+=pow(normalsAtPoint[id][i],2); area=sqrt(area); for(int i=0; i<3; i++) normalsAtPoint[id][i]/=area; } } } double typicalLengthSq=0, lengthSq; clean->GetOutput()->GetPoint(0,point); for(int i=0; i<3; i++) typicalLengthSq+=pow(point[i]-center[i],2); typicalLengthSq/=numberOfCoronas*numberOfCoronas; bool signChange = true; for(int c=1; c<numberOfCoronas; c++) { nptsUntilCorona[c]=nptsUntilCorona[c-1]+nptsInCorona[c-1]; nUsedIds=0; if(this->closeHoleAlgorithm == SMOOTH_PATCH) { /* double displacement=1e10; for(int id=0; id<nptsInCorona[c-1]; id++) { points->GetPoint(nptsUntilCorona[c-1]+id,point); a=0; b=0; mn=0; for(int i=0; i<3; i++) { point[i]-=center[i]; a+=point[i]*normalsAtPoint[id][i]; b+=point[i]*normal[i]; mn+=normalsAtPoint[id][i]*normal[i]; } l=(mn*a-b)/(1-pow(mn,2)); if(firstDisplacement) { if(displacement>l) displacement=l; firstDisplacement = false; } else { if(displacement>l) displacement=l; } } for(int i=0; i<3; i++) { center[i]+=displacement*normal[i]; }*/ for(int i=0; i<3; i++) { center[i]=0; for(int id=0; id<nptsInCorona[c-1]; id++) center[i]+=points->GetPoint(nptsUntilCorona[c-1]+id)[i]; center[i]/=nptsInCorona[c-1]; } } for(int id=0; id<nptsInCorona[c-1]; id++) { points->GetPoint(id+nptsUntilCorona[c-1],point1); points->GetPoint((id+1)%nptsInCorona[c-1]+nptsUntilCorona[c-1],point2); lengthSq=0; for(int i=0; i<3; i++) lengthSq+=pow(point1[i]-point2[i],2); onlyOneCorrespondingPoint[c-1][id]= ((this->closeHoleAlgorithm==LINEAR_TO_CENTER || this->closeHoleAlgorithm==SMOOTH_TO_CENTER ) && lengthSq<typicalLengthSq && (id==0 || !onlyOneCorrespondingPoint[c-1][id-1]) && nptsInCorona[c-1]>3 // Added to correct bad behaviour in extrange contours. ); if(!onlyOneCorrespondingPoint[c-1][id]) { switch(this->closeHoleAlgorithm) { case RADIAL_TRIANGLES : for(int i=0; i<3; i++) { point[i]=(point1[i]*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1); } break; case LINEAR_TO_CENTER : case LINEAR_TO_CENTER_NONREDUCING_TRIANGLES : for(int i=0; i<3; i++) { point[i]=((point1[i]+point2[i])/2*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1); } break; case SMOOTH_TO_CENTER : a=0; l=0; for(int i=0; i<3; i++) { point[i]=(point1[i]+point2[i])/2-center[i]; a+=point[i]*normal[i]; l+=pow(point[i],2); } R=l/a/2; theta=asin(a/sqrt(l)); theta=2*theta*(1.-1./(numberOfCoronas-c+1)); if(fabs(theta)<0.001) { l=pow(a,2)/l; double fractionAngle=1.-1./(numberOfCoronas-c+1); for(int i=0; i<3; i++) { point[i]=pow(fractionAngle,2)*a*normal[i]+center[i] +fractionAngle/sqrt(1-l)*(point[i]-a*normal[i]); } } else { for(int i=0; i<3; i++) { point[i]=-R*cos(theta)*normal[i] +R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normal[i]) +center[i]+R*normal[i]; } } break; case SMOOTH_PATCH : points->GetPoint(nptsUntilCorona[c-1]+id,point); a=0; b=0; mn=0; for(int i=0; i<3; i++) { point[i]-=center[i]; a+=point[i]*normalsAtPoint[id][i]; b+=point[i]*normal[i]; mn+=normalsAtPoint[id][i]*normal[i]; } l=(mn*a-b)/(1-pow(mn,2)); signChange = (l>0); a=0; l=0; b=0; mn=0; for(int i=0; i<3; i++) { point[i]=-(point1[i]+point2[i])/2+center[i]; a+=point[i]*normalsAtPoint[id][i]; l+=pow(point[i],2); } R=l/a/2; theta=asin(a/sqrt(l)); double fractionAngle=1./(numberOfCoronas-c+1); if(signChange) fractionAngle*=-1; theta=2*theta*fractionAngle; if(fabs(theta)<0.001) { l=pow(a,2)/l; for(int i=0; i<3; i++) { point[i]=pow(fractionAngle,2)*a*normalsAtPoint[id][i] +center[i]-point[i] +fractionAngle/sqrt(1-l)*(point[i]-a*normalsAtPoint[id][i]); } } else { for(int i=0; i<3; i++) { point[i]=-R*cos(theta)*normalsAtPoint[id][i] +R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normalsAtPoint[id][i]) +center[i]-point[i]+R*normalsAtPoint[id][i]; } } for(int i=0; i<3; i++) { point1[i]-=point[i]; point2[i]-=point[i]; } for(int i=0; i<3; i++) { normalsAtPoint[nUsedIds][i]=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3]; } area=0; for(int i=0; i<3; i++) area+=pow(normalsAtPoint[nUsedIds][i],2); area=sqrt(area); for(int i=0; i<3; i++) normalsAtPoint[nUsedIds][i]/=area; break; } nUsedIds++; points->InsertNextPoint(point); } } nptsInCorona[c]=nUsedIds; onlyOneCorrespondingPoint[c].resize(nUsedIds); } nptsUntilCorona[numberOfCoronas]=nptsUntilCorona[numberOfCoronas-1]+nptsInCorona[numberOfCoronas-1]; points->InsertNextPoint(center); // polys = vtkCellArray::New(); //A polygon is created for(int c=0; c<numberOfCoronas-1; c++) { int id2=0; for(int id=0; id<nptsInCorona[c] ; id++) { if(onlyOneCorrespondingPoint[c][id]) { ptIds1->Reset(); ptIds1->InsertNextId(id+nptsUntilCorona[c]); ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]); ptIds1->InsertNextId(id2%nptsInCorona[c+1]+nptsUntilCorona[c+1]); polys->InsertNextCell(ptIds1); } else { ptIds1->Reset(); ptIds1->InsertNextId(id+nptsUntilCorona[c]); ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]); ptIds1->InsertNextId(id2+nptsUntilCorona[c+1]); ptIds2->Reset(); ptIds2->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]); ptIds2->InsertNextId(id2+nptsUntilCorona[c+1]); ptIds2->InsertNextId((id2+1)%nptsInCorona[c+1]+nptsUntilCorona[c+1]); polys->InsertNextCell(ptIds1); polys->InsertNextCell(ptIds2); id2++; } } } for(int id=0; id< nptsInCorona[numberOfCoronas-1]; id++) { ptIds1->Reset(); ptIds1->InsertNextId(id+nptsUntilCorona[numberOfCoronas-1]); ptIds1->InsertNextId((id+1)%nptsInCorona[numberOfCoronas-1]+nptsUntilCorona[numberOfCoronas-1]); ptIds1->InsertNextId(nptsUntilCorona[numberOfCoronas]); polys->InsertNextCell(ptIds1); } }
/* * Use Girard's theorem to compute the area of a sky polygon. */ double Girard() { int i, j, ibad; double area; double lon, lat; Vec side[16]; double ang[16]; Vec tmp; double sumang, cosAng, sinAng; sumang = 0; if (nv < 3) return 0; if (DEBUG >= 4) { for (i = 0; i < nv; ++i) { lon = atan2(V[i].y, V[i].x) / DEG_TO_RADIANS; lat = asin(V[i].z) / DEG_TO_RADIANS; printf("Girard(): %3d [%13.6e,%13.6e,%13.6e] -> (%10.6f,%10.6f)\n", i, V[i].x, V[i].y, V[i].z, lon, lat); fflush(stdout); } } for (i = 0; i < nv; ++i) { Cross(&V[i], &V[(i + 1) % nv], &side[i]); Normalize(&side[i]); } for (i = 0; i < nv; ++i) { Cross(&side[i], &side[(i + 1) % nv], &tmp); sinAng = Normalize(&tmp); cosAng = -Dot(&side[i], &side[(i + 1) % nv]); // Remove center point of colinear segments ang[i] = atan2(sinAng, cosAng); if (DEBUG >= 4) { if (i == 0) printf("\n"); printf("Girard(): angle[%d] = %13.6e -> %13.6e (from %13.6e / %13.6e)\n", i, ang[i], ang[i] - M_PI / 2., sinAng, cosAng); fflush(stdout); } // Direction changes of less than a degree can be tricky if (ang[i] > M_PI - 0.0175) { ibad = (i + 1) % nv; if (DEBUG >= 4) { printf("Girard(): ---------- Corner %d bad; " "Remove point %d -------------\n", i, ibad); fflush(stdout); } --nv; for (j = ibad; j < nv; ++j) { V[j].x = V[j + 1].x; V[j].y = V[j + 1].y; V[j].z = V[j + 1].z; } return (Girard()); } sumang += ang[i]; } area = sumang - (nv - 2.) * M_PI; if (mNaN(area) || area < 0.) area = 0.; if (DEBUG >= 4) { printf("\nGirard(): area = %13.6e [%d]\n\n", area, nv); fflush(stdout); } return area; }
/** * \brief Calculate distance between two points * This function uses an algorithm for an oblate spheroid earth model. * The algorithm is described here: * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf * \return Distance in meters */ double nmea_distance_ellipsoid( const nmeaPOS *from_pos, /**< From position in radians */ const nmeaPOS *to_pos, /**< To position in radians */ double *from_azimuth, /**< (O) azimuth at "from" position in radians */ double *to_azimuth /**< (O) azimuth at "to" position in radians */ ) { /* All variables */ double f, a, b, sqr_a, sqr_b; double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; int remaining_steps; double sqr_u, A, B, delta_sigma; /* Check input */ NMEA_ASSERT( from_pos != 0 ); NMEA_ASSERT( to_pos != 0 ); if (( from_pos->lat == to_pos->lat ) && ( from_pos->lon == to_pos->lon ) ) { /* Identical points */ if ( from_azimuth != 0 ) *from_azimuth = 0; if ( to_azimuth != 0 ) *to_azimuth = 0; return 0; } /* Identical points */ /* Earth geometry */ f = NMEA_EARTH_FLATTENING; a = NMEA_EARTH_SEMIMAJORAXIS_M; b = ( 1 - f ) * a; sqr_a = a * a; sqr_b = b * b; /* Calculation */ L = to_pos->lon - from_pos->lon; phi1 = from_pos->lat; phi2 = to_pos->lat; U1 = atan(( 1 - f ) * tan( phi1 ) ); U2 = atan(( 1 - f ) * tan( phi2 ) ); sin_U1 = sin( U1 ); sin_U2 = sin( U2 ); cos_U1 = cos( U1 ); cos_U2 = cos( U2 ); /* Initialize iteration */ sigma = 0; sin_sigma = sin( sigma ); cos_sigma = cos( sigma ); cos_2_sigmam = 0; sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; sqr_cos_alpha = 0; lambda = L; sin_lambda = sin( lambda ); cos_lambda = cos( lambda ); delta_lambda = lambda; remaining_steps = 20; while (( delta_lambda > 1e-12 ) && ( remaining_steps > 0 ) ) { /* Iterate */ /* Variables */ double tmp1, tmp2, sin_alpha, cos_alpha, C, lambda_prev; /* Calculation */ tmp1 = cos_U2 * sin_lambda; tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; sin_sigma = sqrt( tmp1 * tmp1 + tmp2 * tmp2 ); cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; cos_alpha = cos( asin( sin_alpha ) ); sqr_cos_alpha = cos_alpha * cos_alpha; cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; C = f / 16 * sqr_cos_alpha * ( 4 + f * ( 4 - 3 * sqr_cos_alpha ) ); lambda_prev = lambda; sigma = asin( sin_sigma ); lambda = L + ( 1 - C ) * f * sin_alpha * ( sigma + C * sin_sigma * ( cos_2_sigmam + C * cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) ) ); delta_lambda = lambda_prev - lambda; if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; sin_lambda = sin( lambda ); cos_lambda = cos( lambda ); remaining_steps--; } /* Iterate */ /* More calculation */ sqr_u = sqr_cos_alpha * ( sqr_a - sqr_b ) / sqr_b; A = 1 + sqr_u / 16384 * ( 4096 + sqr_u * ( -768 + sqr_u * ( 320 - 175 * sqr_u ) ) ); B = sqr_u / 1024 * ( 256 + sqr_u * ( -128 + sqr_u * ( 74 - 47 * sqr_u ) ) ); delta_sigma = B * sin_sigma * ( cos_2_sigmam + B / 4 * ( cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) - B / 6 * cos_2_sigmam * ( -3 + 4 * sin_sigma * sin_sigma ) * ( -3 + 4 * sqr_cos_2_sigmam ) ) ); /* Calculate result */ if ( from_azimuth != 0 ) { double tan_alpha_1 = cos_U2 * sin_lambda / ( cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda ); *from_azimuth = atan( tan_alpha_1 ); } if ( to_azimuth != 0 ) { double tan_alpha_2 = cos_U1 * sin_lambda / ( -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda ); *to_azimuth = atan( tan_alpha_2 ); } return b * A *( sigma - delta_sigma ); }
void ParticleEffectEntityItem::stepSimulation(float deltaTime) { _particleMinBound = glm::vec3(-1.0f, -1.0f, -1.0f); _particleMaxBound = glm::vec3(1.0f, 1.0f, 1.0f); // update particles between head and tail for (quint32 i = _particleHeadIndex; i != _particleTailIndex; i = (i + 1) % _maxParticles) { _particleLifetimes[i] -= deltaTime; // if particle has died. if (_particleLifetimes[i] <= 0.0f || _lifespan == 0.0f) { // move head forward _particleHeadIndex = (_particleHeadIndex + 1) % _maxParticles; } else { float age = 1.0f - _particleLifetimes[i] / _lifespan; // 0.0 .. 1.0 updateRadius(i, age); updateColor(i, age); updateAlpha(i, age); integrateParticle(i, deltaTime); extendBounds(_particlePositions[i]); } } // emit new particles, but only if we are emmitting if (getIsEmitting() && _emitRate > 0.0f && _lifespan > 0.0f && _polarStart <= _polarFinish) { float timeLeftInFrame = deltaTime; while (_timeUntilNextEmit < timeLeftInFrame) { timeLeftInFrame -= _timeUntilNextEmit; _timeUntilNextEmit = 1.0f / _emitRate; // emit a new particle at tail index. quint32 i = _particleTailIndex; _particleLifetimes[i] = _lifespan; // Radius if (_radiusSpread == 0.0f) { _radiusStarts[i] = getRadiusStart(); _radiusMiddles[i] =_particleRadius; _radiusFinishes[i] = getRadiusFinish(); } else { float spreadMultiplier; if (_particleRadius > 0.0f) { spreadMultiplier = 1.0f + randFloatInRange(-1.0f, 1.0f) * _radiusSpread / _particleRadius; } else { spreadMultiplier = 1.0f; } _radiusStarts[i] = glm::clamp(spreadMultiplier * getRadiusStart(), MINIMUM_PARTICLE_RADIUS, MAXIMUM_PARTICLE_RADIUS); _radiusMiddles[i] = glm::clamp(spreadMultiplier * _particleRadius, MINIMUM_PARTICLE_RADIUS, MAXIMUM_PARTICLE_RADIUS); _radiusFinishes[i] = glm::clamp(spreadMultiplier * getRadiusFinish(), MINIMUM_PARTICLE_RADIUS, MAXIMUM_PARTICLE_RADIUS); } updateRadius(i, 0.0f); // Position, velocity, and acceleration if (_polarStart == 0.0f && _polarFinish == 0.0f && _emitDimensions.z == 0.0f) { // Emit along z-axis from position _particlePositions[i] = getPosition(); _particleVelocities[i] = (_emitSpeed + randFloatInRange(-1.0f, 1.0f) * _speedSpread) * (_emitOrientation * Z_AXIS); _particleAccelerations[i] = _emitAcceleration + randFloatInRange(-1.0f, 1.0f) * _accelerationSpread; } else { // Emit around point or from ellipsoid // - Distribute directions evenly around point // - Distribute points relatively evenly over ellipsoid surface // - Distribute points relatively evenly within ellipsoid volume float elevationMinZ = sin(PI_OVER_TWO - _polarFinish); float elevationMaxZ = sin(PI_OVER_TWO - _polarStart); float elevation = asin(elevationMinZ + (elevationMaxZ - elevationMinZ) * randFloat()); float azimuth; if (_azimuthFinish >= _azimuthStart) { azimuth = _azimuthStart + (_azimuthFinish - _azimuthStart) * randFloat(); } else { azimuth = _azimuthStart + (TWO_PI + _azimuthFinish - _azimuthStart) * randFloat(); } glm::vec3 emitDirection; if (_emitDimensions == glm::vec3()) { // Point emitDirection = glm::quat(glm::vec3(PI_OVER_TWO - elevation, 0.0f, azimuth)) * Z_AXIS; _particlePositions[i] = getPosition(); } else { // Ellipsoid float radiusScale = 1.0f; if (_emitRadiusStart < 1.0f) { float emitRadiusStart = glm::max(_emitRadiusStart, EPSILON); // Avoid math complications at center float randRadius = emitRadiusStart + randFloatInRange(0.0f, MAXIMUM_EMIT_RADIUS_START - emitRadiusStart); radiusScale = 1.0f - std::pow(1.0f - randRadius, 3.0f); } glm::vec3 radiuses = radiusScale * 0.5f * _emitDimensions; float x = radiuses.x * glm::cos(elevation) * glm::cos(azimuth); float y = radiuses.y * glm::cos(elevation) * glm::sin(azimuth); float z = radiuses.z * glm::sin(elevation); glm::vec3 emitPosition = glm::vec3(x, y, z); emitDirection = glm::normalize(glm::vec3( radiuses.x > 0.0f ? x / (radiuses.x * radiuses.x) : 0.0f, radiuses.y > 0.0f ? y / (radiuses.y * radiuses.y) : 0.0f, radiuses.z > 0.0f ? z / (radiuses.z * radiuses.z) : 0.0f )); _particlePositions[i] = getPosition() + _emitOrientation * emitPosition; } _particleVelocities[i] = (_emitSpeed + randFloatInRange(-1.0f, 1.0f) * _speedSpread) * (_emitOrientation * emitDirection); _particleAccelerations[i] = _emitAcceleration + randFloatInRange(-1.0f, 1.0f) * _accelerationSpread; } integrateParticle(i, timeLeftInFrame); extendBounds(_particlePositions[i]); // Color if (_colorSpread == xColor{ 0, 0, 0 }) { _colorStarts[i] = getColorStart(); _colorMiddles[i] = getXColor(); _colorFinishes[i] = getColorFinish(); } else { xColor startColor = getColorStart(); xColor middleColor = getXColor(); xColor finishColor = getColorFinish(); float spread = randFloatInRange(-1.0f, 1.0f); float spreadMultiplierRed = middleColor.red > 0 ? 1.0f + spread * (float)_colorSpread.red / (float)middleColor.red : 1.0f; float spreadMultiplierGreen = middleColor.green > 0 ? 1.0f + spread * (float)_colorSpread.green / (float)middleColor.green : 1.0f; float spreadMultiplierBlue = middleColor.blue > 0 ? 1.0f + spread * (float)_colorSpread.blue / (float)middleColor.blue : 1.0f; _colorStarts[i].red = (int)glm::clamp(spreadMultiplierRed * (float)startColor.red, 0.0f, 255.0f); _colorStarts[i].green = (int)glm::clamp(spreadMultiplierGreen * (float)startColor.green, 0.0f, 255.0f); _colorStarts[i].blue = (int)glm::clamp(spreadMultiplierBlue * (float)startColor.blue, 0.0f, 255.0f); _colorMiddles[i].red = (int)glm::clamp(spreadMultiplierRed * (float)middleColor.red, 0.0f, 255.0f); _colorMiddles[i].green = (int)glm::clamp(spreadMultiplierGreen * (float)middleColor.green, 0.0f, 255.0f); _colorMiddles[i].blue = (int)glm::clamp(spreadMultiplierBlue * (float)middleColor.blue, 0.0f, 255.0f); _colorFinishes[i].red = (int)glm::clamp(spreadMultiplierRed * (float)finishColor.red, 0.0f, 255.0f); _colorFinishes[i].green = (int)glm::clamp(spreadMultiplierGreen * (float)finishColor.green, 0.0f, 255.0f); _colorFinishes[i].blue = (int)glm::clamp(spreadMultiplierBlue * (float)finishColor.blue, 0.0f, 255.0f); } updateColor(i, 0.0f); // Alpha if (_alphaSpread == 0.0f) { _alphaStarts[i] = getAlphaStart(); _alphaMiddles[i] = _alpha; _alphaFinishes[i] = getAlphaFinish(); } else { float spreadMultiplier = 1.0f + randFloatInRange(-1.0f, 1.0f) * _alphaSpread / _alpha; _alphaStarts[i] = spreadMultiplier * getAlphaStart(); _alphaMiddles[i] = spreadMultiplier * _alpha; _alphaFinishes[i] = spreadMultiplier * getAlphaFinish(); } updateAlpha(i, 0.0f); _particleTailIndex = (_particleTailIndex + 1) % _maxParticles; // overflow! move head forward by one. // because the case of head == tail indicates an empty array, not a full one. // This can drop an existing older particle, but this is by design, newer particles are a higher priority. if (_particleTailIndex == _particleHeadIndex) { _particleHeadIndex = (_particleHeadIndex + 1) % _maxParticles; } } _timeUntilNextEmit -= timeLeftInFrame; } }
bool GeoAlgorithms::initVincenty(double aLat1, double aLon1, double aLat2, double aLon2) { // Verify that input latitudes are between -90 and 90 and longitudes are // between -180 and 180 if ((abs(aLat1) > 90) || (abs(aLat2) > 90) || (abs(aLon1) > 180) || (abs(aLon2) > 180)) { return false; } // convert inputs in degrees to radians: aLat1 = aLat1 * 0.0174532925199433; aLon1 = aLon1 * 0.0174532925199433; aLat2 = aLat2 * 0.0174532925199433; aLon2 = aLon2 * 0.0174532925199433; // correct for errors at exact poles by adjusting 0.6 millimeters: if (abs(GeoConversions::PI_OVER_2-abs(aLat1)) < (1e-10)) { aLat1 = getSign(aLat1) * (GeoConversions::PI_OVER_2 - (1e-10)); } if (abs(GeoConversions::PI_OVER_2-abs(aLat2)) < (1e-10)) { aLat2 = getSign(aLat2) * (GeoConversions::PI_OVER_2 - (1e-10)); } // Ellipse CalcuaAltitudeions? mVincentyU1 = atan(m1MinF*tan(aLat1)); mVincentyU2 = atan(m1MinF*tan(aLat2)); aLon1 = getMod(aLon1, (GeoConversions::TWO_PI)); aLon2 = getMod(aLon2, (GeoConversions::TWO_PI)); mVincentyL = aLon2-aLon1; if (abs(mVincentyL) > PI) { mVincentyL = getSign(mVincentyL) * (GeoConversions::TWO_PI - abs(mVincentyL)); } // Initialize Variables for Loop double sin_mVincentyU1 = sin(mVincentyU1); double cos_mVincentyU1 = cos(mVincentyU1); double sin_mVincentyU2 = sin(mVincentyU2); double cos_mVincentyU2 = cos(mVincentyU2); double sinU1_sinU2 = sin_mVincentyU1 * sin_mVincentyU2; double cosU1_sinU2 = cos_mVincentyU1 * sin_mVincentyU2; double sinU1_cosU2 = sin_mVincentyU1 * cos_mVincentyU2; double cosU1_cosU2 = cos_mVincentyU1 * cos_mVincentyU2; double sin_mVincentyLambda = 0; double cos_mVincentyLambda = 0; double cos_mVincentyAlpha = 0; double sin_mVincentySigma = 0; double cos_mVincentySigma = 0; double lLambdaOld = 0; long lIterCount = 0; double lSinSigma = 0; double lCosSigma = 0; double c = 0; mVincentySigma = 0; mVincentyAlpha = 0; mVincentyCosToSigmaM = 0; mVincentyLambda = mVincentyL; // ? while ((!lIterCount) || abs((mVincentyLambda-lLambdaOld) > (1e-12))) { lIterCount += 1; if (lIterCount > 50) { mVincentyLambda = PI; break; } sin_mVincentyLambda = sin(mVincentyLambda); cos_mVincentyLambda = cos(mVincentyLambda); lLambdaOld = mVincentyLambda; lSinSigma = sqrt(pow(cos_mVincentyU2 * sin_mVincentyLambda, 2) + pow(cosU1_sinU2 - sinU1_cosU2 * cos_mVincentyLambda, 2)); lCosSigma = sinU1_sinU2 + cosU1_cosU2 * cos_mVincentyLambda; mVincentySigma = atan2(lSinSigma, lCosSigma); sin_mVincentySigma = sin(mVincentySigma); cos_mVincentySigma = cos(mVincentySigma); mVincentyAlpha = asin(cosU1_cosU2 * sin_mVincentyLambda / sin_mVincentySigma); cos_mVincentyAlpha = cos(mVincentyAlpha); mVincentyCosToSigmaM = cos_mVincentySigma - 2 * sinU1_sinU2 / pow(cos_mVincentyAlpha, 2); c = mF/ 16 * pow(cos_mVincentyAlpha, 2) * (4 + mF * (4 - 3 * pow(cos_mVincentyAlpha, 2))); mVincentyLambda = mVincentyL + (1 - c) * mF * sin(mVincentyAlpha) * (mVincentySigma + c * sin_mVincentySigma * (mVincentyCosToSigmaM + c * cos_mVincentySigma * (-1 + 2 * pow(mVincentyCosToSigmaM, 2)))); // Correct for convergence failure in the case of essentially // antipodal points if (mVincentyLambda > PI) { mVincentyLambda = PI; break; } } return true; }
void Controller_StateMachine_v2::processTurn() { // "during:" // 1º proyectar posicion sobre el plano r_ur, r_ur2 Vector aux_ur2(3), aux_vector(3); aux_vector.copy(&r_ur); jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(r_ur, r_ur2), aux_vector ); aux_ur2.substraction( &r_ur2, &aux_vector); jesus_library::unitarizeVector(aux_ur2); // vector ortogonal a r_ur; para formar base ortonormal en el plano r_ur, r_ur2 Vector pos_act(3); pos_act.setValueData(xei,1); pos_act.setValueData(yei,2); pos_act.setValueData(zei,3); aux_vector.substraction( &pos_act, &c_pc); Vector pos_act_proy_ur(3),pos_act_proy_ur2(3); pos_act_proy_ur.copy(&r_ur); pos_act_proy_ur2.copy(&aux_ur2); jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(r_ur, aux_vector), pos_act_proy_ur ); jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(aux_ur2, aux_vector), pos_act_proy_ur2 ); Vector pos_act_proy(3); pos_act_proy.addition( &pos_act_proy_ur, &pos_act_proy_ur2); cvg_double current_radius = jesus_library::normOfVector(pos_act_proy); #ifdef SM_STATEMACHINE_DEBUG std::cout << "c_pc = \n"; c_pc.mostrar(); std::cout << "c_pinit = \n"; c_pinit.mostrar(); std::cout << "c_pend = \n"; c_pend.mostrar(); std::cout << "pos_act_proy = \n"; pos_act_proy.mostrar(); #endif // SM_STATEMACHINE_DEBUG // 2º localizar pref Vector u_ro(3); u_ro.copy(&pos_act_proy); jesus_library::unitarizeVector(u_ro); aux_vector.copy(&u_ro); jesus_library::multiplyDoubleVsVector( c_Rt, aux_vector); Vector pos_ref(3); pos_ref.addition(&c_pc, &aux_vector); jesus_library::getVectorComponents(pos_ref, xrefo, yrefo, zrefo); cvg_double current_altitude_error = (zei-zrefo); // fabs(zei-zrefo); // 3º calcular velocidad de referencia Vector u_fi(3); jesus_library::crossProduct(u_fi, c_u0, u_ro); jesus_library::unitarizeVector(u_fi); Vector v_ref(3); v_ref.copy(&u_fi); jesus_library::multiplyDoubleVsVector(c_vc, v_ref); jesus_library::getVectorComponents(v_ref, vxfo, vyfo, vzfo); // // 4º Calcular pitcho, rollo derivBlock_vxfo.setInput( vxfo); dvxfo = derivBlock_vxfo.getOutput(); derivBlock_vyfo.setInput( vyfo); dvyfo = derivBlock_vyfo.getOutput(); #ifdef SM_TRAJECTORYMODE_ACTIVATE_TILTFO double g = 9.81; pitchfo = -asin( (cvg_double) dvxfo/g); rollfo = asin( (cvg_double) dvyfo/g); pitchfo *= 1/SM_TRAJECTORYMODE_TILTFO_RAD2TILTREF; rollfo *= 1/SM_TRAJECTORYMODE_TILTFO_RAD2TILTREF; #else pitchfo = 0.0; rollfo = 0.0; #endif // // This is a code that never work that was intended to stop the parrot in it's current position // double v_act = sqrt( vxei*vxei + vyei*vyei + vzei*vzei ); // double act_req = pow(v_act,2)/c_Rt; // double tilt_req = act_req/SM_STATEMACHINE_G; // aux_vector.copy(&u_ro); // jesus_library::multiplyDoubleVsVector( -tilt_req, aux_vector); // aux_vector.setValueData( 0.0, 3); // // Vector parrot_ux(3), parrot_uy(3); // parrot_ux.setValueData( cos(yawei), 1); // parrot_ux.setValueData( sin(yawei), 2); // parrot_uy.setValueData(-sin(yawei), 1); // parrot_uy.setValueData( cos(yawei), 2); // // pitchfo = -jesus_library::dotProduct( parrot_ux, aux_vector)*SM_STATEMACHINE_TILTCOMM_CORRECT_FACTOR; // rollfo = jesus_library::dotProduct( parrot_uy, aux_vector)*SM_STATEMACHINE_TILTCOMM_CORRECT_FACTOR; // pitchfo /= SM_STATEMACHINE_TILTCOMM_NORMALIZATION_CONSTANT; // rollfo /= SM_STATEMACHINE_TILTCOMM_NORMALIZATION_CONSTANT; // 5º Calculo de alpha y comparacion con c_alim Vector aux_vector2(3); aux_vector.substraction(&c_pinit,&c_pc); jesus_library::unitarizeVector(aux_vector); cvg_double aux = jesus_library::dotProduct(u_ro, aux_vector); jesus_library::saturate( aux, -1, 1); jesus_library::crossProduct(aux_vector2, aux_vector, u_ro); cvg_double angle_sign = ( jesus_library::dotProduct(c_u0, aux_vector2) > 0.0 ) ? +1.0 : -1.0; cvg_double current_alpha = angle_sign*acos(aux); #ifdef SM_STATEMACHINE_DEBUG std::cout << "c_alim = " << c_alim << "; alpha = " << current_alpha << "\n"; #endif // SM_STATEMACHINE_DEBUG if ( current_alpha > c_alim) { // ended the turn c_nextState = SM_stateNames::STRAIGHT; c_changeState = true; return; } else { // turn not ended, check safety zones using: current_radius, current_alpha if ( ( current_alpha < trajectory.traj_config.turnmode_safetyzone_negalpha_rad ) || ( fabs(current_radius - c_Rt) > trajectory.traj_config.turnmode_safetyzone_radius_m ) || ( fabs(current_altitude_error) > trajectory.traj_config.turnmode_safetyzone_altitude_m ) ) { // enter hover to prev checkpoint #ifdef SM_STATEMACHINE_DEBUG std::cout << "pos_act_proy_ur < 0\n"; #endif // SM_STATEMACHINE_DEBUG trajectory[pr_checkpoint].convert2Vector(h_checkpoint); c_nextState = SM_stateNames::HOVER; h_stay_in_last_checkpoint = false; c_changeState = true; // In this case I have to redefine the state machine output jesus_library::getVectorComponents(h_checkpoint, xrefo, yrefo, zrefo); vxfo = 0.0; vyfo = 0.0; vzfo = 0.0; pitchfo = 0.0; rollfo = 0.0; return; } else { // Continue turn, nothing else to do return; } } }
// LINE void HTMesh::intersect(double ra1, double dec1, double ra2, double dec2, BufNum bufNum) { double x1, y1, z1, x2, y2, z2; //if (ra1 == 0.0 || ra1 == 180.00) ra1 += 0.1; //if (ra2 == 0.0 || ra2 == 180.00) ra2 -= 0.1; //if (dec1 == 0.0 ) dec1 += 0.1; //if (dec2 == 0.0 ) dec2 -= 0.1; // convert to Cartesian. Ugh. toXYZ( ra1, dec1, &x1, &y1, &z1); toXYZ( ra2, dec2, &x2, &y2, &z2); // Check if points are too close double len; len = fabs(x1 - x2); len += fabs(y1 - y2); len += fabs(z1 - z2); if (htmDebug > 0 ) { printf("htmDebug = %d\n", htmDebug); printf("p1 = (%f, %f, %f)\n", x1, y1, z1); printf("p2 = (%f, %f, %f)\n", x2, y2, z2); printf("edge: %f (radians) %f (degrees)\n", edge, edge / degree2Rad); printf("len : %f (radians) %f (degrees)\n", len, len / degree2Rad); } if ( len < edge10 ) return intersect( ra1, len, bufNum); // Cartesian cross product => perpendicular!. Ugh. double cx = y1 * z2 - z1 * y2; double cy = z1 * x2 - x1 * z2; double cz = x1 * y2 - y1 * x2; if ( htmDebug > 0 ) printf("cp = (%f, %f, %f)\n", cx, cy, cz); double norm = edge10 / ( fabs(cx) + fabs(cy) + fabs(cz) ); // give it length edge/10 cx *= norm; cy *= norm; cz *= norm; if ( htmDebug > 0 ) printf("cpn = (%f, %f, %f)\n", cx, cy, cz); // add it to (ra1, dec1) cx += x1; cy += y1; cz += z1; if ( htmDebug > 0 ) printf("cpf = (%f, %f, %f)\n", cx, cy, cz); // back to spherical norm = sqrt( cx*cx + cy*cy + cz*cz); double ra0 = atan2( cy, cx ) / degree2Rad; double dec0 = asin( cz / norm ) / degree2Rad; if ( htmDebug > 0 ) printf("new ra, dec = (%f, %f)\n", ra0, dec0); SpatialVector p1(ra1, dec1); SpatialVector p0(ra0, dec0); SpatialVector p2(ra2, dec2); RangeConvex convex(&p1, &p0, &p2); if ( ! performIntersection(&convex, bufNum) ) printf("In intersect(%f, %f, %f, %f)\n", ra1, dec1, ra2, dec2); }
template <typename T> inline T acsc(const T &a) { return asin(1 / a); }
void Pager::paintEvent(QPaintEvent * event) { QFrame::paintEvent(event); if (d->images.size() == 0) { return; } // Maximum bounding box of a single page image QSize box = clampedBoundingBox(); // In parametric equation of a circle, the offset from the centre is: // x = a sin t // Spread determines how the index of page maps to the above t. double h = (double) box.height(); double w = (double) box.width(); double a = (width() - w - d->marginLeft - d->marginRight - (d->orientation == Qt::Vertical && d->scrollBar->isVisible() ? d->scrollBar->width() : 0)) / 2.0; double _1 = asin(w/a); double s = spread(); double t1 = _1 + s * ((_PI / 2) - _1); int limitGrace = 5; //int upperLimit = (int) (d->guiIndex + _PI / (2.0 * t1)) + limitGrace + 1; // FIXME shuffle int upperLimit = (int) (d->guiIndex + width() / w + 1); //int lowerLimit = (int) (d->guiIndex - _PI / (2.0 * t1)) - limitGrace; // FIXME shuffle int lowerLimit = (int) (d->guiIndex - width() / w - 1); QPainter painter(this); painter.setClipRect(rect().adjusted(0, 1, -1, 0)); painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::TextAntialiasing, true); painter.setRenderHint(QPainter::SmoothPixmapTransform, true); QFont f(painter.font()); f.setPixelSize(9); painter.setFont(f); painter.translate(width() / 2, 0); painter.translate(-box.width() / 2, 0); // FIXME shuffle double lower = a; double upper = (count() - 1) * w - lower; double clamp = qBound(lower, d->guiIndex * w, upper); painter.translate(-clamp, 0); // end FIXME shuffle painter.translate(0, d->marginTop); painter.setPen(Qt::NoPen); if (isActiveWindow()) { painter.setPen(QApplication::palette().highlight().color()); painter.setBrush(QApplication::palette().highlight()); } else { int gray = qGray(QApplication::palette().highlight().color().rgb()); painter.setPen(QColor(gray, gray, gray)); painter.setBrush(QColor(gray, gray, gray)); } double radius = 4.0; double transientWidth = 0.0; double transientHeight = 0.0; double transientLabelWidth = 0.0; double transientLabelBottom = 0.0; if (d->images.size() == 1) { QSize imageSize = d->images[0].size(); imageSize.scale(box, Qt::KeepAspectRatio); transientWidth = imageSize.width(); transientHeight = imageSize.height(); if (d->drawLabels && !labelAt(0).isEmpty()) { QRect labelRect(QPoint((box.width() - transientWidth) / 2.0, radius), QPoint(box.width() - (box.width() - transientWidth) / 2.0, 12 + radius)); QString elidedLabel = painter.fontMetrics().elidedText(labelAt(0), Qt::ElideMiddle, labelRect.width()); QRect bb(painter.fontMetrics().boundingRect(labelRect, Qt::AlignCenter, elidedLabel)); transientLabelWidth = (double) bb.width(); transientLabelBottom = (double) bb.bottom(); } } else { int previousIndex = clampIndex((int) floor(d->guiIndex)); int nextIndex = clampIndex((int) ceil(d->guiIndex)); QSize previousSize = box; if (!d->images[previousIndex].isNull()) { previousSize = d->images[previousIndex].size(); previousSize.scale(box, Qt::KeepAspectRatio); } QSize nextSize = box; if (!d->images[nextIndex].isNull()) { nextSize = d->images[nextIndex].size(); nextSize.scale(box, Qt::KeepAspectRatio); } transientWidth = previousSize.width() + (nextSize.width() - previousSize.width()) * (d->guiIndex - previousIndex); transientHeight = previousSize.height() + (nextSize.height() - previousSize.height()) * (d->guiIndex - previousIndex); if (d->drawLabels && !labelAt(previousIndex).isEmpty() && !labelAt(nextIndex).isEmpty()) { double previousLabelWidth = 0.0; double previousLabelBottom = 0.0; { QRect labelRect(0, box.height() + radius, previousSize.width(), 12); QString elidedLabel = painter.fontMetrics().elidedText(labelAt(previousIndex), Qt::ElideMiddle, labelRect.width()); QRect bb(painter.fontMetrics().boundingRect(labelRect, Qt::AlignCenter, elidedLabel)); previousLabelWidth = (double) bb.width(); previousLabelBottom = (double) bb.bottom(); } double nextLabelWidth = 0.0; double nextLabelBottom = 0.0; { QRect labelRect(0, box.height() + radius, nextSize.width(), 12); QString elidedLabel = painter.fontMetrics().elidedText(labelAt(nextIndex), Qt::ElideMiddle, labelRect.width()); QRect bb(painter.fontMetrics().boundingRect(labelRect, Qt::AlignCenter, elidedLabel)); nextLabelWidth = (double) bb.width(); nextLabelBottom = (double) bb.bottom(); } transientLabelWidth = previousLabelWidth + (nextLabelWidth - previousLabelWidth) * (d->guiIndex - previousIndex); transientLabelBottom = previousLabelBottom + (nextLabelBottom - previousLabelBottom) * (d->guiIndex - previousIndex); } } painter.drawRoundedRect(QRectF(d->guiIndex * w + (box.width() - transientWidth) / 2.0 - radius, box.height() - transientHeight - radius, transientWidth + radius * 2, transientHeight + radius * 2), radius, radius); if (d->drawLabels && transientLabelWidth > 0.0 && transientLabelBottom > 0.0) { painter.drawRoundedRect(QRectF(d->guiIndex * w + (box.width() - transientLabelWidth) / 2.0 - radius, box.height(), transientLabelWidth + radius * 2, transientLabelBottom - box.height() + radius / 2.0), radius, radius); } painter.setPen(Qt::NoPen); QPen pen(QColor(115, 115, 115)); pen.setJoinStyle(Qt::MiterJoin); pen.setWidthF(0.5); painter.setPen(pen); painter.setBrush(Qt::NoBrush); static QVector< double > sin_ts; // FIXME Make a member, not a static variable sin_ts.resize(count()); // Iterate through pages double offset = 0.0; for (int i = (int) ceil(d->guiIndex); i < count(); ++i) { if (i < lowerLimit || i > upperLimit) continue; double preModifier = d->indexPreModifiers.value(i, 0.0); double postModifier = d->indexPostModifiers.value(i, 0.0); offset += preModifier; if (preModifier >= 0.0) { double t = (i - (d->guiIndex - offset)) * t1; t = qBound(-_PI/2.0, t, _PI/2); sin_ts[i] = sin(t); } else { sin_ts[i] = 10.0; } offset += postModifier; } offset = 0.0; for (int i = (int) ceil(d->guiIndex) - 1; i >= 0; --i) { if (i < lowerLimit || i > upperLimit) continue; double preModifier = d->indexPreModifiers.value(i, 0.0); double postModifier = d->indexPostModifiers.value(i, 0.0); offset += postModifier; if (preModifier >= 0.0) { double t = (i - (d->guiIndex + offset)) * t1; t = qBound(-_PI/2.0, t, _PI/2); sin_ts[i] = sin(t); } else { sin_ts[i] = 10.0; } offset += preModifier; } for (int i = 0; i < d->guiIndex; ++i) { if (i < lowerLimit || i > upperLimit) continue; double sin_t = sin_ts.at(i); if (sin_t > 1.0) continue; double opacity = 1.0; if (d->transitionTimes.contains(i) && !d->transitionTimes[i].isNull()) { opacity *= d->transitionTimes[i].elapsed() / 500.0; } if (i < lowerLimit + limitGrace) { // opacity *= (i - lowerLimit) / (double) limitGrace; // FIXME shuffle } painter.setOpacity(opacity); QPixmap image = d->images[i].isNull() ? QPixmap(":/images/pager_loading.png") : d->images[i]; QSize size = image.size(); size.scale(box, Qt::KeepAspectRatio); //double x = sin_t * (a + (w - size.width()) / 2.0) + (w - size.width()) / 2.0; // FIXME shuffle double x = w * i + (w - size.width()) / 2.0; double y = h - size.height(); QRect rect((int) x, (int) y, size.width(), size.height()); painter.drawPixmap(rect, image); painter.drawRect(rect); if (d->drawLabels) { QString label = labelAt(i); if (!label.isEmpty()) { QRect labelRect(rect.bottomLeft() + QPoint(0, radius), rect.bottomRight() + QPoint(0, 12 + radius)); QString elidedLabel = painter.fontMetrics().elidedText(label, Qt::ElideMiddle, rect.width()); painter.save(); painter.setPen(QColor(0, 0, 0)); QFontMetrics fm(painter.font()); QSize numSize = fm.boundingRect(elidedLabel).size(); double lozengeRadius = 1 + numSize.height() / 2.0; numSize += QSize(numSize.height(), 0); QRect numRect(0, 0, numSize.width() + 2, numSize.height() + 2); numRect.moveCenter(rect.center()); numRect.moveBottom(rect.bottom() + radius + 1); painter.setBrush(Qt::yellow); painter.setPen(QColor(140, 140, 0)); painter.drawRoundedRect(numRect, lozengeRadius, lozengeRadius); painter.setPen(Qt::black); painter.drawText(numRect, Qt::AlignCenter, elidedLabel); painter.restore(); } } if (d->hasAnnotation.value(i, 0) > 0) { drawAnnotationTag(&painter, rect); } if (!d->spotlightsHidden && !d->searchHits.isEmpty()) { drawSearchHits(&painter, d->searchHits.value(i, 0), rect); } painter.setOpacity(1.0); } for (int i = count() - 1; i >= d->guiIndex; --i) { if (i < lowerLimit || i > upperLimit) continue; double sin_t = sin_ts.at(i); if (sin_t > 1.0) continue; double opacity = 1.0; if (d->transitionTimes.contains(i) && !d->transitionTimes[i].isNull()) { opacity *= d->transitionTimes[i].elapsed() / 500.0; } if (i > upperLimit - limitGrace) { // opacity *= (limitGrace + i - upperLimit) / (double) limitGrace; // FIXME shuffle } painter.setOpacity(opacity); QPixmap image = d->images[i].isNull() ? QPixmap(":/images/pager_loading.png") : d->images[i]; QSize size = image.size(); size.scale(box, Qt::KeepAspectRatio); //double x = sin_t * (a + (w - size.width()) / 2.0) + (w - size.width()) / 2.0; // FIXME shuffle double x = w * i + (w - size.width()) / 2.0; double y = h - size.height(); QRect rect((int) x, (int) y, size.width(), size.height()); painter.drawPixmap(rect, image); painter.drawRect(rect); if (d->drawLabels) { QString label = labelAt(i); if (!label.isEmpty()) { QRect labelRect(rect.bottomLeft() + QPoint(0, radius), rect.bottomRight() + QPoint(0, 12 + radius)); QString elidedLabel = painter.fontMetrics().elidedText(label, Qt::ElideMiddle, labelRect.width()); painter.save(); painter.setPen(QColor(200, 200, 200)); painter.setPen(QColor(0, 0, 0)); painter.drawText(labelRect, Qt::AlignCenter, elidedLabel); painter.restore(); } } if (d->hasAnnotation.value(i, 0) > 0) { drawAnnotationTag(&painter, rect); } if (!d->spotlightsHidden && !d->searchHits.isEmpty()) { drawSearchHits(&painter, d->searchHits.value(i, 0), rect); } painter.setOpacity(1.0); } sin_ts.clear(); }
void XYZ2GEOM(double X, double Y, double Z, double *Lon, double *Lat) { *Lon = asin(Z / sqrt(X * X + Y * Y + Z * Z)) * 180 / M_PI; *Lat = atan2(Y, X) * 180 / M_PI; }
static int math_asin (lua_State *L) { lua_pushnumber(L, asin(luaL_checknumber(L, 1))); return 1; }
static void fcn_asin(EvalState *c) { c->rStackPtr->v.value = asin(c->rStackPtr->v.value); }
MINLINE float saasin(float fac) { if(fac<= -1.0f) return (float)-M_PI/2.0f; else if(fac>=1.0f) return (float)M_PI/2.0f; else return (float)asin(fac); }
void LibAsin(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { ReturnValue->Val->FP = asin(Param[0]->Val->FP); }
bool TScriptInternalFunctions::runFunction(QString function, QStringList param, QString &result) { QString fn = function.toUpper(); if (fn == "ACOS") { if (param.length() < 1) return false; bool ok = false; double v = param[0].toDouble(&ok); if (!ok) v = 0; result = QString::number(acos(v)); return true; } if (fn == "ASIN") { if (param.length() < 1) return false; bool ok = false; double v = param[0].toDouble(&ok); if (!ok) v = 0; result = QString::number(asin(v)); return true; } if (fn == "ATAN") { if (param.length() < 1) return false; bool ok = false; double v = param[0].toDouble(&ok); if (!ok) v = 0; result = QString::number(atan(v)); return true; } if (fn == "BUILDTYPE") { #ifdef STANDALONE result = "STANDALONE"; #endif #ifdef PACKAGED result = "PACKAGED"; #endif return true; } if (fn == "CALC") { // Calculate an expression (e.g. 5+5) if (param.length() == 0) return false; QString expr = param[0]; result = calc(expr); return true; } if (fn == "COS") { if (param.length() < 1) return false; bool ok = false; double v = param[0].toDouble(&ok); if (!ok) v = 0; result = QString::number(cos(v)); return true; } if (fn == "COLORAT") { // $ColorAt(@window, layer, x, y) if (param.length() < 3) return false; QString layer = "main"; if (param.length() > 3) { layer = param[1]; param.removeAt(1); } subwindow_t sw = getCustomWindow(param[0]); if (sw.type == WT_NOTHING) return false; int x = floor( param[1].toFloat() ); int y = floor( param[2].toFloat() ); result = sw.widget->picwinPtr()->colorAt(layer, x, y); return true; } if (fn == "CURWINTYPE") { // Returns the current target type (msg or channel) subwindow_t sw = winList->value(*activeWid); if (sw.type == WT_CHANNEL) result = "CHANNEL"; else if (sw.type == WT_GRAPHIC) result = "GRAPHIC"; else if (sw.type == WT_GWINPUT) result = "GRAPHICINPUT"; else if (sw.type == WT_NOTHING) result = "NOTHIG"; else if (sw.type == WT_PRIVMSG) result = "PRIVMSG"; else if (sw.type == WT_STATUS) result = "STATUS"; else if (sw.type == WT_TXTONLY) result = "TXTONLY"; else result = "UNKNOWN"; return true; } if (fn == "DLG") { // Returns information of a dialog or its objects // $dlg(dialog,object) if (param.count() == 2) { QString dlg = param[0]; QString object = param[1]; QHashIterator<QString,TCustomScriptDialog*> i(*dialogs); while (i.hasNext()) { i.next(); if (i.key().toUpper() == dlg.toUpper()) { result = i.value()->getLabel(object); return true; } } } // $dlg(dialog,object,index) if (param.count() == 3) { QString dlg = param[0]; QString object = param[1]; QString index = param[2]; QHashIterator<QString,TCustomScriptDialog*> i(*dialogs); while (i.hasNext()) { i.next(); if (i.key().toUpper() == dlg.toUpper()) { result = i.value()->getItem(object, index.toInt()); return true; } } } // Default return false; } if (fn == "FILE") { // Returns a file descriptor by opening a file for read and|or write // $file(file.name, rwb) // result: 0 cannot open, -1 not existing if (param.count() < 2) { result = "0"; return true; } QString mode = param[1]; bool read = false; bool write = false; bool binary = false; bool append = false; bool switchfail = false; for (int i = 0; i <= mode.length()-1; i++) { char c = mode[i].toLatin1(); switch (c) { case 'a': append = true; continue; case 'r': read = true; continue; case 'w': write = true; continue; case 'b': binary = true; continue; default: switchfail = true; break; } } if (switchfail == true) { result = "0"; return true; } if ((read || write) == false) read = true; QIODevice::OpenMode om = 0; if (read) om |= QIODevice::ReadOnly; if (write) om |= QIODevice::WriteOnly; if (! binary) om |= QIODevice::Text; if (append) om |= QIODevice::Append; if (om == 0) { result = "0"; return true; } QFile *f = new QFile(param[0]); if (! f->open(om)) { result = "0"; return true; } t_sfile ts; ts.binary = binary; ts.read = read; ts.write = write; ts.fd = fdc; ts.file = f; files->insert(fdc, ts); result = QString::number(fdc++); return true; } if (fn == "FNEXIST") { // Checks if an actual function exists; This will NOT work on "internal" functions (these in here) if (param.count() != 1) return false; int idx = fnindex->value(param[0].toUpper(), -1); if (idx > -1) result = "1"; else result = "0"; return true; } if (fn == "GLUE") { // "glue" texts together. // $glue(hello,big,world) will return hellobigworld QString r; for (int i = 0; i <= param.length()-1; i++) r += param[i]; result = r; return true; } if (fn == "HOSTMASK") { // Returns hostmask *!*@host.name of nickname if IAL got it. // Otherwise, if IAL doesn't, it returns nickname!*@* as hostmask. if (param.count() != 1) { result.clear(); return false; } QString nickname = param[0]; IConnection *con = conList->value(*activeConn); QString host = con->ial.getHost(nickname); if (host.isEmpty()) { host = nickname; host.append("!*@*"); } else host.prepend("*!*@"); result = host; return true; } if (fn == "IALHOSTMASK") { // Returns hostname of nickname if IAL got it, otherwise empty text. // Better off using $hostmask() instead. if (param.count() != 1) { result.clear(); return false; } QString nickname = param[0]; IConnection *con = conList->value(*activeConn); QString host = con->ial.getHost(nickname); if (! host.isEmpty()) host.prepend("*!*@"); result = host; return true; } if (fn == "LEN") { // Counts amount of letters in a given text if (param.count() == 0) { result = "0"; return true; } result = QString::number( param.at(0).length() ); return true; } if (fn == "NULL") { // Returns empty result.clear(); return true; } if (fn == "PATH") { // $path(type) // Returns a file path to the given type. if (param.count() != 1) return false; QString type = param[0].toUpper(); if (type == "CONFIG") result = CONF_PATH; if (type == "COMMON") result = COMMON_PATH; if (type == "EXEC") result = QApplication::applicationDirPath(); if (type == "SKEL") result = SKEL_PATH; return true; } if (fn == "RAND") { // Pseudo-random number generator if (param.length() < 2) return false; int lo = param[0].toInt(); int hi = param[1].toInt(); result = rand(lo, hi); return true; } if (fn == "SIN") { if (param.length() < 1) return false; bool ok = false; double v = param[0].toDouble(&ok); if (!ok) v = 0; result = QString::number(sin(v)); return true; } if (fn == "SOCKBUFLEN") { // Returns amount of bytes left in sockread buffer if (param.length() < 1) return false; result = sockfactory->sockBufLen(param[0]); return true; } if (fn == "SOCKLIST") { // Find socket names. // $socklist(patt_*, pos) // If pos is zero, that will return amount of socket names the pattern matches. // If pos > 0, this will return an actual socket name it matches. if (param.length() < 2) return false; result = sockfactory->socklist(param[0], param[1].toInt()); return true; } if (fn == "SSTR") { // Substring, returns text by given positions inside the text. // $SSTR(The text, start, end) end is optional. if (param.length() < 2) return false; int start = param[1].toInt(); int stop = -1; if (param.length() >= 3) stop = param[2].toInt(); QString text = param[0]; result = sstr(text, start, stop); return true; } if (fn == "TAN") { if (param.length() < 1) return false; bool ok = false; double v = param[0].toDouble(&ok); if (!ok) v = 0; result = QString::number(tan(v)); return true; } if (fn == "TARGET") { // Returns the current target to send messages to (msg or channel) subwindow_t sw = winList->value(*activeWid); result.clear(); if ((sw.type == WT_CHANNEL) || (sw.type == WT_PRIVMSG)) result = sw.widget->getTarget(); return true; } if (fn == "TEXTWIDTH") { // Return text width in pixles by given font name and size // $textwidth(font, size, text) if (param.count() != 3) return false; QFont font(param[0]); font.setPixelSize(param[1].toInt()); QFontMetrics fm(font); result = QString::number( fm.width(param[2]) ); return true; } if (fn == "TOKEN") { // Get a text by tokens // $token(text here, position, token) the token is in ascii number. // If the position is zero, this will count amount of text items separated by the given token if (param.length() < 3) return false; bool ok = false; QString tcnum = param[2]; // token character (ascii num) QChar tc = tcnum.toInt(&ok); // converted from string-number to actual number, into a character. if (ok == false) return false; int p = param[1].toInt(&ok); // Which position to use if (ok == false) return false; result = token(param[0], p, tc); return true; } if (fn == "VERSION") { // IIRC version. result = VERSION_STRING; return true; } // No functions were matching, return false as error. return false; }
CAMLprim value caml_asin_float_r(CAML_R, value f) { return caml_copy_double_r(ctx,asin(Double_val(f))); }
double QgsDistanceArea::computeDistanceBearing( const QgsPoint& p1, const QgsPoint& p2, double* course1, double* course2 ) { if ( p1.x() == p2.x() && p1.y() == p2.y() ) return 0; // ellipsoid double a = mSemiMajor; double b = mSemiMinor; double f = 1 / mInvFlattening; double p1_lat = DEG2RAD( p1.y() ), p1_lon = DEG2RAD( p1.x() ); double p2_lat = DEG2RAD( p2.y() ), p2_lon = DEG2RAD( p2.x() ); double L = p2_lon - p1_lon; double U1 = atan(( 1 - f ) * tan( p1_lat ) ); double U2 = atan(( 1 - f ) * tan( p2_lat ) ); double sinU1 = sin( U1 ), cosU1 = cos( U1 ); double sinU2 = sin( U2 ), cosU2 = cos( U2 ); double lambda = L; double lambdaP = 2 * M_PI; double sinLambda = 0; double cosLambda = 0; double sinSigma = 0; double cosSigma = 0; double sigma = 0; double alpha = 0; double cosSqAlpha = 0; double cos2SigmaM = 0; double C = 0; double tu1 = 0; double tu2 = 0; int iterLimit = 20; while ( qAbs( lambda - lambdaP ) > 1e-12 && --iterLimit > 0 ) { sinLambda = sin( lambda ); cosLambda = cos( lambda ); tu1 = ( cosU2 * sinLambda ); tu2 = ( cosU1 * sinU2 - sinU1 * cosU2 * cosLambda ); sinSigma = sqrt( tu1 * tu1 + tu2 * tu2 ); cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda; sigma = atan2( sinSigma, cosSigma ); alpha = asin( cosU1 * cosU2 * sinLambda / sinSigma ); cosSqAlpha = cos( alpha ) * cos( alpha ); cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha; C = f / 16 * cosSqAlpha * ( 4 + f * ( 4 - 3 * cosSqAlpha ) ); lambdaP = lambda; lambda = L + ( 1 - C ) * f * sin( alpha ) * ( sigma + C * sinSigma * ( cos2SigmaM + C * cosSigma * ( -1 + 2 * cos2SigmaM * cos2SigmaM ) ) ); } if ( iterLimit == 0 ) return -1; // formula failed to converge double uSq = cosSqAlpha * ( a * a - b * b ) / ( b * b ); double A = 1 + uSq / 16384 * ( 4096 + uSq * ( -768 + uSq * ( 320 - 175 * uSq ) ) ); double B = uSq / 1024 * ( 256 + uSq * ( -128 + uSq * ( 74 - 47 * uSq ) ) ); double deltaSigma = B * sinSigma * ( cos2SigmaM + B / 4 * ( cosSigma * ( -1 + 2 * cos2SigmaM * cos2SigmaM ) - B / 6 * cos2SigmaM * ( -3 + 4 * sinSigma * sinSigma ) * ( -3 + 4 * cos2SigmaM * cos2SigmaM ) ) ); double s = b * A * ( sigma - deltaSigma ); if ( course1 ) { *course1 = atan2( tu1, tu2 ); } if ( course2 ) { // PI is added to return azimuth from P2 to P1 *course2 = atan2( cosU1 * sinLambda, -sinU1 * cosU2 + cosU1 * sinU2 * cosLambda ) + M_PI; } return s; }
void HairpinSegment::layout() { if (hairpin()->useTextLine()) { if (parent()) rypos() += score()->styleS(StyleIdx::hairpinY).val() * spatium(); TextLineSegment::layout(); return; } QTransform t; qreal _spatium = spatium(); qreal h1 = hairpin()->hairpinHeight().val() * spatium() * .5; qreal h2 = hairpin()->hairpinContHeight().val() * spatium() * .5; qreal len; qreal x = pos2().x(); if (x < _spatium) // minimum size of hairpin x = _spatium; qreal y = pos2().y(); len = sqrt(x * x + y * y); t.rotateRadians(asin(y/len)); drawCircledTip = hairpin()->hairpinCircledTip(); circledTipRadius = 0; if( drawCircledTip ) circledTipRadius = 0.6 * _spatium * .5; if (hairpin()->hairpinType() == Hairpin::Type::CRESCENDO) { // crescendo switch (spannerSegmentType()) { case SpannerSegmentType::SINGLE: case SpannerSegmentType::BEGIN: l1.setLine(.0 + circledTipRadius*2, .0, len, h1); l2.setLine(.0 + circledTipRadius*2, .0, len, -h1); circledTip.setX( 0 + circledTipRadius ); circledTip.setY( 0 ); break; case SpannerSegmentType::MIDDLE: case SpannerSegmentType::END: drawCircledTip = false; l1.setLine(.0, h2, len, h1); l2.setLine(.0, -h2, len, -h1); break; } } else { // decrescendo switch(spannerSegmentType()) { case SpannerSegmentType::SINGLE: case SpannerSegmentType::END: l1.setLine(.0, h1, len - circledTipRadius*2, 0.0); l2.setLine(.0, -h1, len - circledTipRadius*2, 0.0); circledTip.setX( len - circledTipRadius ); circledTip.setY( 0 ); break; case SpannerSegmentType::BEGIN: case SpannerSegmentType::MIDDLE: drawCircledTip = false; l1.setLine(.0, h1, len, + h2); l2.setLine(.0, -h1, len, - h2); break; } } // Do Coord rotation l1 = t.map(l1); l2 = t.map(l2); if( drawCircledTip ) circledTip = t.map(circledTip); QRectF r = QRectF(l1.p1(), l1.p2()).normalized() | QRectF(l2.p1(), l2.p2()).normalized(); qreal w = point(score()->styleS(StyleIdx::hairpinLineWidth)); setbbox(r.adjusted(-w*.5, -w*.5, w, w)); if (parent()) rypos() += score()->styleS(StyleIdx::hairpinY).val() * _spatium; adjustReadPos(); }
CAAPhysicalJupiterDetails CAAPhysicalJupiter::Calculate(double JD) { //What will be the return value CAAPhysicalJupiterDetails details; //Step 1 double d = JD - 2433282.5; double T1 = d/36525; double alpha0 = 268.00 + 0.1061*T1; double alpha0rad = CAACoordinateTransformation::DegreesToRadians(alpha0); double delta0 = 64.50 - 0.0164*T1; double delta0rad = CAACoordinateTransformation::DegreesToRadians(delta0); //Step 2 double W1 = CAACoordinateTransformation::MapTo0To360Range(17.710 + 877.90003539*d); double W2 = CAACoordinateTransformation::MapTo0To360Range(16.838 + 870.27003539*d); //Step 3 double l0 = CAAEarth::EclipticLongitude(JD); double l0rad = CAACoordinateTransformation::DegreesToRadians(l0); double b0 = CAAEarth::EclipticLatitude(JD); double b0rad = CAACoordinateTransformation::DegreesToRadians(b0); double R = CAAEarth::RadiusVector(JD); //Step 4 double l = CAAJupiter::EclipticLongitude(JD); double lrad = CAACoordinateTransformation::DegreesToRadians(l); double b = CAAJupiter::EclipticLatitude(JD); double brad = CAACoordinateTransformation::DegreesToRadians(b); double r = CAAJupiter::RadiusVector(JD); //Step 5 double x = r*cos(brad)*cos(lrad) - R*cos(l0rad); double y = r*cos(brad)*sin(lrad) - R*sin(l0rad); double z = r*sin(brad) - R*sin(b0rad); double DELTA = sqrt(x*x + y*y + z*z); //Step 6 l -= 0.012990*DELTA/(r*r); lrad = CAACoordinateTransformation::DegreesToRadians(l); //Step 7 x = r*cos(brad)*cos(lrad) - R*cos(l0rad); y = r*cos(brad)*sin(lrad) - R*sin(l0rad); z = r*sin(brad) - R*sin(b0rad); DELTA = sqrt(x*x + y*y + z*z); //Step 8 double e0 = CAANutation::MeanObliquityOfEcliptic(JD); double e0rad = CAACoordinateTransformation::DegreesToRadians(e0); //Step 9 double alphas = atan2(cos(e0rad)*sin(lrad) - sin(e0rad)*tan(brad), cos(lrad)); double deltas = asin(cos(e0rad)*sin(brad) + sin(e0rad)*cos(brad)*sin(lrad)); //Step 10 details.DS = CAACoordinateTransformation::RadiansToDegrees(asin(-sin(delta0rad)*sin(deltas) - cos(delta0rad)*cos(deltas)*cos(alpha0rad - alphas))); //Step 11 double u = y*cos(e0rad) - z*sin(e0rad); double v = y*sin(e0rad) + z*cos(e0rad); double alpharad = atan2(u, x); double alpha = CAACoordinateTransformation::RadiansToDegrees(alpharad); double deltarad = atan2(v, sqrt(x*x + u*u)); double delta = CAACoordinateTransformation::RadiansToDegrees(deltarad); double xi = atan2(sin(delta0rad)*cos(deltarad)*cos(alpha0rad - alpharad) - sin(deltarad)*cos(delta0rad), cos(deltarad)*sin(alpha0rad - alpharad)); //Step 12 details.DE = CAACoordinateTransformation::RadiansToDegrees(asin(-sin(delta0rad)*sin(deltarad) - cos(delta0rad)*cos(deltarad)*cos(alpha0rad - alpharad))); //Step 13 details.Geometricw1 = CAACoordinateTransformation::MapTo0To360Range(W1 - CAACoordinateTransformation::RadiansToDegrees(xi) - 5.07033*DELTA); details.Geometricw2 = CAACoordinateTransformation::MapTo0To360Range(W2 - CAACoordinateTransformation::RadiansToDegrees(xi) - 5.02626*DELTA); //Step 14 double C = 57.2958 * (2*r*DELTA + R*R - r*r - DELTA*DELTA)/(4*r*DELTA); if (sin(lrad - l0rad) > 0) { details.Apparentw1 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw1 + C); details.Apparentw2 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw2 + C); } else { details.Apparentw1 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw1 - C); details.Apparentw2 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw2 - C); } //Step 15 double NutationInLongitude = CAANutation::NutationInLongitude(JD); double NutationInObliquity = CAANutation::NutationInObliquity(JD); e0 += NutationInObliquity/3600; e0rad = CAACoordinateTransformation::DegreesToRadians(e0); //Step 16 alpha += 0.005693*(cos(alpharad)*cos(l0rad)*cos(e0rad) + sin(alpharad)*sin(l0rad))/cos(deltarad); alpha = CAACoordinateTransformation::MapTo0To360Range(alpha); alpharad = CAACoordinateTransformation::DegreesToRadians(alpha); delta += 0.005693*(cos(l0rad)*cos(e0rad)*(tan(e0rad)*cos(deltarad) - sin(alpharad)*sin(deltarad)) + cos(alpharad)*sin(deltarad)*sin(l0rad)); deltarad = CAACoordinateTransformation::DegreesToRadians(delta); //Step 17 double NutationRA = CAANutation::NutationInRightAscension(alpha/15, delta, e0, NutationInLongitude, NutationInObliquity); double alphadash = alpha + NutationRA/3600; double alphadashrad = CAACoordinateTransformation::DegreesToRadians(alphadash); double NutationDec = CAANutation::NutationInDeclination(alpha/15, delta, e0, NutationInLongitude, NutationInObliquity); double deltadash = delta + NutationDec/3600; double deltadashrad = CAACoordinateTransformation::DegreesToRadians(deltadash); NutationRA = CAANutation::NutationInRightAscension(alpha0/15, delta0, e0, NutationInLongitude, NutationInObliquity); double alpha0dash = alpha0 + NutationRA/3600; double alpha0dashrad = CAACoordinateTransformation::DegreesToRadians(alpha0dash); NutationDec = CAANutation::NutationInDeclination(alpha0/15, delta0, e0, NutationInLongitude, NutationInObliquity); double delta0dash = delta0 + NutationDec/3600; double delta0dashrad = CAACoordinateTransformation::DegreesToRadians(delta0dash); //Step 18 details.P = CAACoordinateTransformation::MapTo0To360Range(CAACoordinateTransformation::RadiansToDegrees(atan2(cos(delta0dashrad)*sin(alpha0dashrad - alphadashrad), sin(delta0dashrad)*cos(deltadashrad) - cos(delta0dashrad)*sin(deltadashrad)*cos(alpha0dashrad - alphadashrad)))); return details; }
void pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) { // Check if input was set if (!normals_) { PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); output.width = output.height = 0; output.points.clear (); return; } if (normals_->points.size () != surface_->points.size ()) { PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ()); output.width = output.height = 0; output.points.clear (); return; } Eigen::Vector3f plane_normal; plane_normal[0] = -centroid_[0]; plane_normal[1] = -centroid_[1]; plane_normal[2] = -centroid_[2]; Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); plane_normal.normalize (); Eigen::Vector3f axis = plane_normal.cross (z_vector); double rotation = -asin (axis.norm ()); axis.normalize (); int nbins = nbins_; int bin_angle = 360 / nbins; Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis)); pcl::PointCloud<pcl::PointNormal> grid; grid.points.resize (indices_->size ()); for (size_t i = 0; i < indices_->size (); i++) { grid.points[i].getVector4fMap () = surface_->points[(*indices_)[i]].getVector4fMap (); grid.points[i].getNormalVector4fMap () = normals_->points[(*indices_)[i]].getNormalVector4fMap (); } pcl::transformPointCloudWithNormals (grid, grid, transformPC); //fill spatial data vector and the zero-initialize or "value-initialize" an array on c++, // the initialization is made with () after the [nbins] kiss_fft_scalar * spatial_data = new kiss_fft_scalar[nbins](); float sum_w = 0, w = 0; int bin = 0; for (const auto &point : grid.points) { bin = static_cast<int> ((((atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins; w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x); sum_w += w; spatial_data[bin] += w; } for (int i = 0; i < nbins; ++i) spatial_data[i] /= sum_w; kiss_fft_cpx * freq_data = new kiss_fft_cpx[nbins / 2 + 1]; kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, NULL, NULL); kiss_fftr (mycfg, spatial_data, freq_data); output.points.resize (1); output.width = output.height = 1; output.points[0].histogram[0] = freq_data[0].r / freq_data[0].r; //dc int k = 1; for (int i = 1; i < (nbins / 2); i++, k += 2) { output.points[0].histogram[k] = freq_data[i].r / freq_data[0].r; output.points[0].histogram[k + 1] = freq_data[i].i / freq_data[0].r; } output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r / freq_data[0].r; //nyquist delete[] spatial_data; delete[] freq_data; }
kmScalar kmQuaternionGetYaw(const kmQuaternion* q) { float result = asin(-2 * (q->x * q->z - q->w * q->y)); return result; }
void MadgwickAHRSupdate(float gx,float gy,float gz,float ax,float ay,float az,float mx,float my,float mz, attitude_t * angle) { float recipNorm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; float qa, qb, qc; // Auxiliary variables to avoid repeated arithmetic q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; float hx, hy, bx, bz; float halfwx, halfwy, halfwz; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Reference direction of Earth's magnetic field hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); bx = sqrt(hx * hx + hy * hy); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); // Estimated direction of magnetic field halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); // Error is sum of cross product between estimated direction and measured direction of field vectors halfex = (my * halfwz - mz * halfwy); halfey = (mz * halfwx - mx * halfwz); halfez = (mx * halfwy - my * halfwx); // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) { float halfvx, halfvy, halfvz; // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity halfvx = q1q3 - q0q2; halfvy = q0q1 + q2q3; halfvz = q0q0 - 0.5f + q3q3; // Error is sum of cross product between estimated direction and measured direction of field vectors halfex += (ay * halfvz - az * halfvy); halfey += (az * halfvx - ax * halfvz); halfez += (ax * halfvy - ay * halfvx); } // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki integralFBy += twoKi * halfey * (1.0f / sampleFreq); integralFBz += twoKi * halfez * (1.0f / sampleFreq); gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKp * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors gy *= (0.5f * (1.0f / sampleFreq)); gz *= (0.5f * (1.0f / sampleFreq)); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; /* output data */ angle->Y = atan2(2 * q1 * q2 + 2 * q0 * q3, q0*q0+q1*q1-q2*q2-q3*q3)* 57.3; angle->P = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitcho??? angle->R = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; }
static void traverse_loop(struct loop *lp, struct connection *anchor_connection) /* * This is the workhorse of the display program. The algorithm is * recursive based on processing individual loops. Each base pairing * region is displayed using the direction given by the circle diagram, * and the connections between the regions is drawn by equally spaced * points. The radius of the loop is set to minimize the square error * for lengths between sequential bases in the loops. The "correct" * length for base links is 1. If the least squares fitting of the * radius results in loops being less than 1/2 unit apart, then that * segment is extruded. * * The variable, anchor_connection, gives the connection to the loop * processed in an previous level of recursion. */ { double xs,ys,xe,ye,xn,yn,angleinc,r; double radius,xc,yc,xo,yo,astart,aend,a; struct connection *cp,*cpnext,**cpp,*acp,*cpprev; int i,j,n,ic; double da,maxang; int count,icstart,icend,icmiddle,icroot; logical done,done_all_connections,rooted; int sign; double midx,midy,nrx,nry,mx,my,vx,vy,dotmv,nmidx,nmidy; int icstart1,icup,icdown,icnext,direction; double dan,dx,dy,rr; double cpx,cpy,cpnextx,cpnexty,cnx,cny,rcn,rc,lnx,lny,rl,ac,acn,sx,sy,dcp; int imaxloop; angleinc = 2 * pi / (nbase+1); acp = NULL; icroot = -1; for (cpp=lp->connections, ic = 0; (cp = *cpp); cpp++, ic++) { /* xs = cos(angleinc*cp->start); ys = sin(angleinc*cp->start); xe = cos(angleinc*cp->end); ye = sin(angleinc*cp->end); */ xs = -sin(angleinc*cp->start); ys = cos(angleinc*cp->start); xe = -sin(angleinc*cp->end); ye = cos(angleinc*cp->end); xn = ye-ys; yn = xs-xe; r = sqrt(xn*xn + yn*yn); cp->xrad = xn/r; cp->yrad = yn/r; cp->angle = atan2(yn,xn); if (cp->angle < 0.0) cp->angle += 2*pi; if (anchor_connection != NULL && anchor_connection->region == cp->region) { acp = cp; icroot = ic; } } set_radius: determine_radius(lp,lencut); radius = lp->radius; if (anchor_connection == NULL) xc = yc = 0.0; else { xo = (bases[acp->start].x+bases[acp->end].x) / 2.0; yo = (bases[acp->start].y+bases[acp->end].y) / 2.0; xc = xo - radius * acp->xrad; yc = yo - radius * acp->yrad; } /* * The construction of the connectors will proceed in blocks of * connected connectors, where a connected connector pairs means * two connectors that are forced out of the drawn circle because they * are too close together in angle. */ /* * First, find the start of a block of connected connectors */ if (icroot == -1) icstart = 0; else icstart = icroot; cp = lp->connections[icstart]; count = 0; if (debug) printf("Now processing loop %d\n",lp->number); done = false; do { j = icstart - 1; if (j < 0) j = lp->nconnection - 1; cpprev = lp->connections[j]; if (!connected_connection(cpprev,cp)) { done = true; } else { icstart = j; cp = cpprev; } if (++count > lp->nconnection) { /* * Here everything is connected. Break on maximum angular separation * between connections. */ maxang = -1.0; for (ic = 0; ic < lp->nconnection; ic++) { j = ic + 1; if (j >= lp->nconnection) j = 0; cp = lp->connections[ic]; cpnext = lp->connections[j]; ac = cpnext->angle - cp->angle; if (ac < 0.0) ac += 2*pi; if (ac > maxang) { maxang = ac; imaxloop = ic; } } icend = imaxloop; icstart = imaxloop + 1; if (icstart >= lp->nconnection) icstart = 0; cp = lp->connections[icend]; cp->broken = true; done = true; } } while (!done); done_all_connections = false; icstart1 = icstart; if (debug) printf("Icstart1 = %d\n",icstart1); while (!done_all_connections) { count = 0; done = false; icend = icstart; rooted = false; while (!done) { cp = lp->connections[icend]; if (icend == icroot) rooted = true; j = icend + 1; if (j >= lp->nconnection) { j = 0; } cpnext = lp->connections[j]; if (connected_connection(cp,cpnext)) { if (++count >= lp->nconnection) break; icend = j; } else { done = true; } } icmiddle = find_ic_middle(icstart,icend,anchor_connection,acp,lp); ic = icup = icdown = icmiddle; if (debug) printf("IC start = %d middle = %d end = %d\n", icstart,icmiddle,icend); done = false; direction = 0; while (!done) { if (direction < 0) { ic = icup; } else if (direction == 0) { ic = icmiddle; } else { ic = icdown; } if (ic >= 0) { cp = lp->connections[ic]; if (anchor_connection == NULL || acp != cp) { if (direction == 0) { astart = cp->angle - asin(1.0/2.0/radius); aend = cp->angle + asin(1.0/2.0/radius); bases[cp->start].x = xc + radius*cos(astart); bases[cp->start].y = yc + radius*sin(astart); bases[cp->end].x = xc + radius*cos(aend); bases[cp->end].y = yc + radius*sin(aend); } else if (direction < 0) { j = ic + 1; if (j >= lp->nconnection) j = 0; cp = lp->connections[ic]; cpnext = lp->connections[j]; cpx = cp->xrad; cpy = cp->yrad; ac = (cp->angle + cpnext->angle) / 2.0; if (cp->angle > cpnext->angle) ac -= pi; cnx = cos(ac); cny = sin(ac); lnx = cny; lny = -cnx; da = cpnext->angle - cp->angle; if (da < 0.0) da += 2*pi; if (cp->extruded) { if (da <= pi/2) rl = 2.0; else rl = 1.5; } else { rl = 1.0; } bases[cp->end].x = bases[cpnext->start].x + rl*lnx; bases[cp->end].y = bases[cpnext->start].y + rl*lny; bases[cp->start].x = bases[cp->end].x + cpy; bases[cp->start].y = bases[cp->end].y - cpx; } else { j = ic - 1; if (j < 0) j = lp->nconnection - 1; cp = lp->connections[j]; cpnext = lp->connections[ic]; cpnextx = cpnext->xrad; cpnexty = cpnext->yrad; ac = (cp->angle + cpnext->angle) / 2.0; if (cp->angle > cpnext->angle) ac -= pi; cnx = cos(ac); cny = sin(ac); lnx = -cny; lny = cnx; da = cpnext->angle - cp->angle; if (da < 0.0) da += 2*pi; if (cp->extruded) { if (da <= pi/2) rl = 2.0; else rl = 1.5; } else { rl = 1.0; } bases[cpnext->start].x = bases[cp->end].x + rl*lnx; bases[cpnext->start].y = bases[cp->end].y + rl*lny; bases[cpnext->end].x = bases[cpnext->start].x - cpnexty; bases[cpnext->end].y = bases[cpnext->start].y + cpnextx; } } } if (direction < 0) { if (icdown == icend) { icdown = -1; } else if (icdown >= 0) { if (++icdown >= lp->nconnection) { icdown = 0; } } direction = 1; } else { if (icup == icstart) icup = -1; else if (icup >= 0) { if (--icup < 0) { icup = lp->nconnection - 1; } } direction = -1; } done = icup == -1 && icdown == -1; } icnext = icend + 1; if (icnext >= lp->nconnection) icnext = 0; if (icend != icstart && (! (icstart == icstart1 && icnext == icstart1))) { /* * Move the bases just constructed (or the radius) so * that the bisector of the end points is radius distance * away from the loop center. */ cp = lp->connections[icstart]; cpnext = lp->connections[icend]; dx = bases[cpnext->end].x - bases[cp->start].x; dy = bases[cpnext->end].y - bases[cp->start].y; midx = bases[cp->start].x + dx/2.0; midy = bases[cp->start].y + dy/2.0; rr = sqrt(dx*dx + dy*dy); mx = dx / rr; my = dy / rr; vx = xc - midx; vy = yc - midy; rr = sqrt(dx*dx + dy*dy); vx /= rr; vy /= rr; dotmv = vx*mx + vy*my; nrx = dotmv*mx - vx; nry = dotmv*my - vy; rr = sqrt(nrx*nrx + nry*nry); nrx /= rr; nry /= rr; /* * Determine which side of the bisector the center should be. */ dx = bases[cp->start].x - xc; dy = bases[cp->start].y - yc; ac = atan2(dy,dx); if (ac < 0.0) ac += 2*pi; dx = bases[cpnext->end].x - xc; dy = bases[cpnext->end].y - yc; acn = atan2(dy,dx); if (acn < 0.0) acn += 2*pi; if (acn < ac) acn += 2*pi; if (acn - ac > pi) sign = -1; else sign = 1; nmidx = xc + sign*radius*nrx; nmidy = yc + sign*radius*nry; if (rooted) { xc -= nmidx - midx; yc -= nmidy - midy; } else { for (ic=icstart; ; ++ic >= lp->nconnection ? (ic = 0) : 0) { cp = lp->connections[ic]; i = cp->start; bases[i].x += nmidx - midx; bases[i].y += nmidy - midy; i = cp->end; bases[i].x += nmidx - midx; bases[i].y += nmidy - midy; if (ic == icend) break; } } } icstart = icnext; done_all_connections = icstart == icstart1; } for (ic=0; ic < lp->nconnection; ic++) { cp = lp->connections[ic]; j = ic + 1; if (j >= lp->nconnection) j = 0; cpnext = lp->connections[j]; dx = bases[cp->end].x - xc; dy = bases[cp->end].y - yc; rc = sqrt(dx*dx + dy*dy); ac = atan2(dy,dx); if (ac < 0.0) ac += 2*pi; dx = bases[cpnext->start].x - xc; dy = bases[cpnext->start].y - yc; rcn = sqrt(dx*dx + dy*dy); acn = atan2(dy,dx); if (acn < 0.0) acn += 2*pi; if (acn < ac) acn += 2*pi; dan = acn - ac; dcp = cpnext->angle - cp->angle; if (dcp <= 0.0) dcp += 2*pi; if (fabs(dan-dcp) > pi) { if (cp->extruded) { fprintf(stderr, "Warning from traverse_loop. Loop %d has crossed regions\n", lp->number); } else if ((cpnext->start - cp->end) != 1) { cp->extruded = true; goto set_radius; /* Forever shamed */ } } if (cp->extruded) { construct_extruded_segment(cp,cpnext); } else { n = cpnext->start - cp->end; if (n < 0) n += nbase + 1; angleinc = dan / n; for (j = 1; j < n; j++) { i = cp->end + j; if (i > nbase) i -= nbase + 1; a = ac + j*angleinc; rr = rc + (rcn-rc)*(a-ac)/dan; bases[i].x = xc + rr*cos(a); bases[i].y = yc + rr*sin(a); } } } for (ic=0; ic < lp->nconnection; ic++) { if (icroot != ic) { cp = lp->connections[ic]; generate_region(cp); traverse_loop(cp->loop,cp); } } n = 0; sx = 0.0; sy = 0.0; for (ic = 0; ic < lp->nconnection; ic++) { j = ic + 1; if (j >= lp->nconnection) j = 0; cp = lp->connections[ic]; cpnext = lp->connections[j]; n += 2; sx += bases[cp->start].x + bases[cp->end].x; sy += bases[cp->start].y + bases[cp->end].y; if (!cp->extruded) { for (j = cp->end + 1; j != cpnext->start; j++) { if (j > nbase) j -= nbase + 1; n++; sx += bases[j].x; sy += bases[j].y; } } } lp->x = sx / n; lp->y = sy / n; /* free connections (ih) */ for (ic = 0; ic < lp->nconnection; ic++) free(lp->connections[ic]); free(lp->connections); }
inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const { lp_lat = pj_authlat(asin( 2. * xy_y * this->m_par.k0 / this->m_proj_parm.qp), this->m_proj_parm.apa); lp_lon = xy_x / this->m_par.k0; }
void RangeSteering::cycleDock(ros::Duration dt) { ros::Time currentTime = ros::Time::now(); double w0, w; geometry_msgs::Twist vel; vel.linear.y = 0.0; vel.linear.z = 0.0; vel.angular.x = 0.0; vel.angular.y = 0.0; //~ /* Check if close enough to start local docking */ //~ double dockStartRange = sqrt(dockStart*dockStart + baseDist*baseDist/4); //~ //~ //~ if (fabs(leftRange - dockStartRange) < 0.1 && fabs(rightRange - dockStartRange) < 0.1) //~ { //~ dockStarted = true; //~ cout << "Starting Local Docking!" << endl; //~ } switch (dockState) { /* Set the turn angle to base, heading is calculated based on the followed radius */ /* Approximated by 90 degrees (actually dependent on radius and both ranges) */ case STATE_START_TURN_TO: { vel.linear.x = 0.0; /* Heading dependent on distance */ double beta = -followDirection * (M_PI / 2 - asin(baseDist / (2 * leftRange))); homeHeading = odoHeading - beta; dockState = STATE_TURN; cout << "Start turning home!, beta: " << beta << ", home: " << homeHeading << ", odoHeading: " << odoHeading << endl; break; } case STATE_TURN: { vel.linear.x = 0.0; vel.angular.z = followDirection * turnSpeed; double diff = fabs(odoHeading - homeHeading); if (diff >= 2 * M_PI) { diff -= 2 * M_PI; } else if (diff <= -2 * M_PI) { diff += 2 * M_PI; } cout << "----Turning diff: " << diff * 180 / M_PI << "---" << endl; /* Stop within +-5 deg of final angle */ if (diff < 5 * M_PI / 180) { vel.angular.z = 0; dockState = STATE_DOCK_STRAIGHT; dockStarted = true; cout << "----Turning done, start straight! " << endl; } cmdPub.publish(vel); break; } case STATE_DOCK_STRAIGHT: { if (dockStarted) { w = 0; //~ printf("cosA: %f, xL: %f, yL: %f, dx: %f, dy: %f, distToHome: %f, angleToHome: %f\n", cosAlpha, xL, yL, //dx,dy, distToHome, angleToHome); /* Check if docking complete */ if (distToHome < 0.1) { cout << "Docking Completed!" << endl; doDock = false; dockStarted = false; vel.linear.x = 0.0; vel.angular.z = 0.0; cmdPub.publish(vel); } if (doDock) { vel.linear.x = 0.2; // speed*(distToHome/dockStart); errDock = angleToHome; if (errOldDock != errDock) { dErrDock = errDock - errOldDock; } errOldDock = errDock; w = kpDock * errDock + kdDock * dErrDock; vel.angular.z = w; cout << "e = " << errDock << ", dErrDock = " << dErrDock << ", w = " << w << ", kpDock*err = " << kpDock* errDock << ", kdDock*dErrDock = " << kdDock* dErrDock << endl; cmdPub.publish(vel); } break; } } } }
//--------------------------------------------------------------------------- float ofxKinect::getAccelRoll(){ return ofRadToDeg(asin(getMksAccel().x/OFX_KINECT_GRAVITY)); }
/* * Check the vertex list for adjacent pairs of * points which are too close together for the * subsequent dot- and cross-product calculations * of Girard's theorem. */ void RemoveDups() { int i, nvnew; Vec Vnew[16]; Vec tmp; double lon, lat; double separation; if (DEBUG >= 4) { printf("RemoveDups() TOLERANCE = %13.6e [%13.6e arcsec]\n\n", TOLERANCE, TOLERANCE / DEG_TO_RADIANS * 3600.); for (i = 0; i < nv; ++i) { lon = atan2(V[i].y, V[i].x) / DEG_TO_RADIANS; lat = asin(V[i].z) / DEG_TO_RADIANS; printf("RemoveDups() orig: %3d [%13.6e,%13.6e,%13.6e] " "-> (%10.6f,%10.6f)\n", i, V[i].x, V[i].y, V[i].z, lon, lat); fflush(stdout); } printf("\n"); } Vnew[0].x = V[0].x; Vnew[0].y = V[0].y; Vnew[0].z = V[0].z; nvnew = 0; for (i = 0; i < nv; ++i) { ++nvnew; Vnew[nvnew].x = V[(i + 1) % nv].x; Vnew[nvnew].y = V[(i + 1) % nv].y; Vnew[nvnew].z = V[(i + 1) % nv].z; Cross(&V[i], &V[(i + 1) % nv], &tmp); separation = Normalize(&tmp); if (DEBUG >= 4) { printf("RemoveDups(): %3d x %3d: distance = %13.6e " "[%13.6e arcsec] (would become %d)\n", (i + 1) % nv, i, separation, separation / DEG_TO_RADIANS * 3600., nvnew); fflush(stdout); } if (separation < TOLERANCE) { --nvnew; if (DEBUG >= 4) { printf("RemoveDups(): %3d is a duplicate (nvnew -> %d)\n", i, nvnew); fflush(stdout); } } } if (DEBUG >= 4) { printf("\n"); fflush(stdout); } if (nvnew < nv) { for (i = 0; i < nvnew; ++i) { V[i].x = Vnew[i].x; V[i].y = Vnew[i].y; V[i].z = Vnew[i].z; } nv = nvnew; } }
static void node_shader_exec_math(void *UNUSED(data), int UNUSED(thread), bNode *node, bNodeExecData *UNUSED(execdata), bNodeStack **in, bNodeStack **out) { switch (node->custom1) { case 0: /* Add */ out[0]->vec[0] = in[0]->vec[0] + in[1]->vec[0]; break; case 1: /* Subtract */ out[0]->vec[0] = in[0]->vec[0] - in[1]->vec[0]; break; case 2: /* Multiply */ out[0]->vec[0] = in[0]->vec[0] * in[1]->vec[0]; break; case 3: /* Divide */ { if (in[1]->vec[0]==0) /* We don't want to divide by zero. */ out[0]->vec[0] = 0.0; else out[0]->vec[0] = in[0]->vec[0] / in[1]->vec[0]; } break; case 4: /* Sine */ { if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */ out[0]->vec[0] = sin(in[0]->vec[0]); else out[0]->vec[0] = sin(in[1]->vec[0]); } break; case 5: /* Cosine */ { if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */ out[0]->vec[0] = cos(in[0]->vec[0]); else out[0]->vec[0] = cos(in[1]->vec[0]); } break; case 6: /* Tangent */ { if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */ out[0]->vec[0] = tan(in[0]->vec[0]); else out[0]->vec[0] = tan(in[1]->vec[0]); } break; case 7: /* Arc-Sine */ { if (in[0]->hasinput || !in[1]->hasinput) { /* This one only takes one input, so we've got to choose. */ /* Can't do the impossible... */ if (in[0]->vec[0] <= 1 && in[0]->vec[0] >= -1) out[0]->vec[0] = asin(in[0]->vec[0]); else out[0]->vec[0] = 0.0; } else { /* Can't do the impossible... */ if (in[1]->vec[0] <= 1 && in[1]->vec[0] >= -1) out[0]->vec[0] = asin(in[1]->vec[0]); else out[0]->vec[0] = 0.0; } } break; case 8: /* Arc-Cosine */ { if (in[0]->hasinput || !in[1]->hasinput) { /* This one only takes one input, so we've got to choose. */ /* Can't do the impossible... */ if (in[0]->vec[0] <= 1 && in[0]->vec[0] >= -1) out[0]->vec[0] = acos(in[0]->vec[0]); else out[0]->vec[0] = 0.0; } else { /* Can't do the impossible... */ if (in[1]->vec[0] <= 1 && in[1]->vec[0] >= -1) out[0]->vec[0] = acos(in[1]->vec[0]); else out[0]->vec[0] = 0.0; } } break; case 9: /* Arc-Tangent */ { if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */ out[0]->vec[0] = atan(in[0]->vec[0]); else out[0]->vec[0] = atan(in[1]->vec[0]); } break; case 10: /* Power */ { /* Only raise negative numbers by full integers */ if (in[0]->vec[0] >= 0) { out[0]->vec[0] = pow(in[0]->vec[0], in[1]->vec[0]); } else { float y_mod_1 = fabsf(fmodf(in[1]->vec[0], 1.0f)); /* if input value is not nearly an integer, fall back to zero, nicer than straight rounding */ if (y_mod_1 > 0.999f || y_mod_1 < 0.001f) { out[0]->vec[0] = powf(in[0]->vec[0], floorf(in[1]->vec[0] + 0.5f)); } else { out[0]->vec[0] = 0.0f; } } } break; case 11: /* Logarithm */ { /* Don't want any imaginary numbers... */ if (in[0]->vec[0] > 0 && in[1]->vec[0] > 0) out[0]->vec[0] = log(in[0]->vec[0]) / log(in[1]->vec[0]); else out[0]->vec[0] = 0.0; } break; case 12: /* Minimum */ { if (in[0]->vec[0] < in[1]->vec[0]) out[0]->vec[0] = in[0]->vec[0]; else out[0]->vec[0] = in[1]->vec[0]; } break; case 13: /* Maximum */ { if (in[0]->vec[0] > in[1]->vec[0]) out[0]->vec[0] = in[0]->vec[0]; else out[0]->vec[0] = in[1]->vec[0]; } break; case 14: /* Round */ { if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */ out[0]->vec[0] = (in[0]->vec[0] < 0) ? (int)(in[0]->vec[0] - 0.5f) : (int)(in[0]->vec[0] + 0.5f); else out[0]->vec[0] = (in[1]->vec[0] < 0) ? (int)(in[1]->vec[0] - 0.5f) : (int)(in[1]->vec[0] + 0.5f); } break; case 15: /* Less Than */ { if (in[0]->vec[0] < in[1]->vec[0]) out[0]->vec[0] = 1.0f; else out[0]->vec[0] = 0.0f; } break; case 16: /* Greater Than */ { if (in[0]->vec[0] > in[1]->vec[0]) out[0]->vec[0] = 1.0f; else out[0]->vec[0] = 0.0f; } break; case 17: /* Modulo */ { if (in[1]->vec[0] == 0.0f) out[0]->vec[0] = 0.0f; else out[0]->vec[0] = fmod(in[0]->vec[0], in[1]->vec[0]); } break; } }
void test_extra(T) { T t = 1; t = abs(t); t = abs(t*t); t = fabs(t); t = fabs(t*t); t = sqrt(t); t = sqrt(t*t); t = floor(t); t = floor(t*t); t = ceil(t); t = ceil(t*t); t = trunc(t); t = trunc(t*t); t = round(t); t = round(t*t); t = exp(t); t = exp(t*t); t = log(t); t = log(t*t); t = log10(t); t = log10(t*t); t = cos(t); t = cos(t*t); t = sin(t); t = sin(t*t); t = tan(t); t = tan(t*t); t = asin(t); t = asin(t*t); t = atan(t); t = atan(t*t); t = acos(t); t = acos(t*t); t = cosh(t); t = cosh(t*t); t = sinh(t); t = sinh(t*t); t = tanh(t); t = tanh(t*t); double dval = 2; t = pow(t, t); t = pow(t, t*t); t = pow(t, dval); t = pow(t*t, t); t = pow(t*t, t*t); t = pow(t*t, dval); t = pow(dval, t); t = pow(dval, t*t); t = atan2(t, t); t = atan2(t, t*t); t = atan2(t, dval); t = atan2(t*t, t); t = atan2(t*t, t*t); t = atan2(t*t, dval); t = atan2(dval, t); t = atan2(dval, t*t); t = fmod(t, t); t = fmod(t, t*t); t = fmod(t, dval); t = fmod(t*t, t); t = fmod(t*t, t*t); t = fmod(t*t, dval); t = fmod(dval, t); t = fmod(dval, t*t); typedef typename T::backend_type backend_type; typedef typename backend_type::exponent_type exp_type; exp_type e = 0; int i = 0; t = ldexp(t, i); t = ldexp(t*t, i); t = ldexp(t, e); t = ldexp(t*t, e); t = frexp(t, &i); t = frexp(t*t, &i); t = frexp(t, &e); t = frexp(t*t, &e); t = scalbn(t, i); t = scalbn(t*t, i); t = scalbn(t, e); t = scalbn(t*t, e); t = logb(t); t = logb(t*t); e = ilogb(t); e = ilogb(t*t); }
void meCloseHoles::Update() { vtkPolyDataNormals * normals = vtkPolyDataNormals::New(); normals->SetInput(this->input); normals->SplittingOff(); normals->SetFeatureAngle(180.0); normals->ComputePointNormalsOff(); normals->ComputeCellNormalsOn(); normals->AutoOrientNormalsOn(); normals->ConsistencyOn(); normals->Update(); normals->GetOutput()->BuildLinks(); vtkPointLocator * locator = vtkPointLocator::New(); locator->SetDataSet(this->input); locator->BuildLocator(); vtkFeatureEdges * edges = vtkFeatureEdges::New(); edges->SetInput(normals->GetOutput()); edges->BoundaryEdgesOn(); edges->FeatureEdgesOff(); edges->ManifoldEdgesOff(); edges->NonManifoldEdgesOff(); edges->Update(); //PolyDataConnectivityFilter: each hole is identified as a region vtkPolyDataConnectivityFilter *connec = vtkPolyDataConnectivityFilter::New(); connec->SetInput(edges->GetOutput()); connec->SetExtractionModeToSpecifiedRegions(); connec->Update(); edges->Delete(); int numEdgesInRegion = 0, numRegions = connec->GetNumberOfExtractedRegions(), nUsedIds, pointId, pointId1, pointId2; this->patchOutput.clear(); patchOutput.resize(numRegions); vtkCleanPolyData * clean = vtkCleanPolyData::New(); vector<vtkPolyData*> poly(numRegions); vtkPolyData * patch; vtkPoints * points; vtkCellArray * polys; vtkIdList * ptIds = vtkIdList::New(), * ptIds1 = vtkIdList::New(), * ptIds2 = vtkIdList::New(), * ptIdsOrdered = vtkIdList::New(), * cellIds = vtkIdList::New(), * cellIdsOrdered = vtkIdList::New(); vector<int> nptsInCorona, nptsUntilCorona; vector< vector<bool> > onlyOneCorrespondingPoint; vector< vector<double> > normalsAtPoint; //For each hole (region) the points are stored and sorted for(int idRegion = 0; idRegion < numRegions; idRegion++ ) { connec->AddSpecifiedRegion(idRegion); connec->Update(); numEdgesInRegion = connec->GetOutput()->GetNumberOfLines(); clean->SetInput(connec->GetOutput()); clean->Update(); //For each hole, point and connectivity lists (not sorted) are created. int npts = clean->GetOutput()->GetNumberOfPoints(); clean->GetOutput()->BuildLinks(); //in order to sort clean->GetOutput()->BuildCells(); clean->GetOutput()->Update(); double center[3]={0,0,0}, point[3], point1[3], point2[3]; points = vtkPoints::New(); ptIdsOrdered->Reset(); cellIdsOrdered->Reset(); int idUsed = 0; for(int id=0; id< numEdgesInRegion; id++) { cellIdsOrdered->InsertNextId(idUsed); clean->GetOutput()->GetCellPoints(idUsed,ptIds); ptIdsOrdered->InsertNextId(ptIds->GetId(0)); clean->GetOutput()->GetPointCells(ptIds->GetId(1),cellIds); idUsed = (cellIds->GetId(0) == idUsed ? cellIds->GetId(1) : idUsed=cellIds->GetId(0)); clean->GetOutput()->GetPoint(ptIds->GetId(0),point); points->InsertNextPoint(point); } for(int id=0; id<npts; id++) { clean->GetOutput()->GetPoint(id,point); for(int i=0; i<3; i++) center[i]+=point[i]; } for(int i=0; i<3; i++) center[i]/=npts; double normal[3]={0,0,0}; double area=0, theta, R, a, l, b, mn; if(this->closeHoleAlgorithm == SMOOTH_TO_CENTER || this->closeHoleAlgorithm == SMOOTH_PATCH) { points->GetPoint(npts-1,point1); for(int i=0; i<3; i++) point1[i]-=center[i]; for(int id=0; id< npts; id++) { points->GetPoint(id,point); for(int i=0; i<3; i++) { point[i]-=center[i]; point2[i]=point[i]-point1[i]; } for(int i=0; i<3; i++) normal[i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3]; for(int i=0; i<3; i++) point1[i]=point2[i]; } for(int i=0; i<3; i++) area+=pow(normal[i],2); area=sqrt(area); for(int i=0; i<3; i++) normal[i]/=area; } int numberOfCoronas=( this->closeHoleAlgorithm==SINGLE_CENTER_POINT ? 1 : npts/6+1 ); if(this->closeHoleAlgorithm == SMOOTH_PATCH) numberOfCoronas*=5; nptsInCorona.clear(); nptsInCorona.resize(numberOfCoronas); nptsUntilCorona.clear(); nptsUntilCorona.resize(numberOfCoronas+1); nptsUntilCorona[0]=0; nptsInCorona[0]=npts; onlyOneCorrespondingPoint.clear(); onlyOneCorrespondingPoint.resize(numberOfCoronas); onlyOneCorrespondingPoint[0].resize(npts); if(this->closeHoleAlgorithm == SMOOTH_PATCH) { normalsAtPoint.resize(npts); for(int id=0; id<npts; id++) { normalsAtPoint[id].clear(); normalsAtPoint[id].resize(3); points->GetPoint(id,point); pointId=locator->FindClosestPoint(point); normals->GetOutput()->GetPointCells(pointId,cellIds); // for(int i=0; i<3; i++) normalsAtPoint[id][i]=0; a=cellIds->GetNumberOfIds(); for(int j=0; j<a; j++) { normals->GetOutput()->GetCellPoints(cellIds->GetId(j),ptIds); int sameId; if(ptIds->GetId(0)==pointId) sameId=0; else if(ptIds->GetId(1)==pointId) sameId=1; else sameId=2; this->input->GetPoint(ptIds->GetId((sameId+1)%3),point1); this->input->GetPoint(ptIds->GetId((sameId+2)%3),point2); for(int i=0; i<3; i++) { point1[i]-=point[i]; point2[i]-=point[i]; } for(int i=0; i<3; i++) { normalsAtPoint[id][i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3]; } area=0; for(int i=0; i<3; i++) area+=pow(normalsAtPoint[id][i],2); area=sqrt(area); for(int i=0; i<3; i++) normalsAtPoint[id][i]/=area; } } } double typicalLengthSq=0, lengthSq; clean->GetOutput()->GetPoint(0,point); for(int i=0; i<3; i++) typicalLengthSq+=pow(point[i]-center[i],2); typicalLengthSq/=numberOfCoronas*numberOfCoronas; bool signChange = true; for(int c=1; c<numberOfCoronas; c++) { nptsUntilCorona[c]=nptsUntilCorona[c-1]+nptsInCorona[c-1]; nUsedIds=0; if(this->closeHoleAlgorithm == SMOOTH_PATCH) { /* double displacement=1e10; for(int id=0; id<nptsInCorona[c-1]; id++) { points->GetPoint(nptsUntilCorona[c-1]+id,point); a=0; b=0; mn=0; for(int i=0; i<3; i++) { point[i]-=center[i]; a+=point[i]*normalsAtPoint[id][i]; b+=point[i]*normal[i]; mn+=normalsAtPoint[id][i]*normal[i]; } l=(mn*a-b)/(1-pow(mn,2)); if(firstDisplacement) { if(displacement>l) displacement=l; firstDisplacement = false; } else { if(displacement>l) displacement=l; } } for(int i=0; i<3; i++) { center[i]+=displacement*normal[i]; }*/ for(int i=0; i<3; i++) { center[i]=0; for(int id=0; id<nptsInCorona[c-1]; id++) center[i]+=points->GetPoint(nptsUntilCorona[c-1]+id)[i]; center[i]/=nptsInCorona[c-1]; } } for(int id=0; id<nptsInCorona[c-1]; id++) { points->GetPoint(id+nptsUntilCorona[c-1],point1); points->GetPoint((id+1)%nptsInCorona[c-1]+nptsUntilCorona[c-1],point2); lengthSq=0; for(int i=0; i<3; i++) lengthSq+=pow(point1[i]-point2[i],2); onlyOneCorrespondingPoint[c-1][id]= ((this->closeHoleAlgorithm==LINEAR_TO_CENTER || this->closeHoleAlgorithm==SMOOTH_TO_CENTER ) && lengthSq<typicalLengthSq && (id==0 || !onlyOneCorrespondingPoint[c-1][id-1]) && nptsInCorona[c-1]>3 // Added to correct bad behaviour in extrange contours. ); if(!onlyOneCorrespondingPoint[c-1][id]) { switch(this->closeHoleAlgorithm) { case RADIAL_TRIANGLES : for(int i=0; i<3; i++) { point[i]=(point1[i]*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1); } break; case LINEAR_TO_CENTER : case LINEAR_TO_CENTER_NONREDUCING_TRIANGLES : for(int i=0; i<3; i++) { point[i]=((point1[i]+point2[i])/2*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1); } break; case SMOOTH_TO_CENTER : a=0; l=0; for(int i=0; i<3; i++) { point[i]=(point1[i]+point2[i])/2-center[i]; a+=point[i]*normal[i]; l+=pow(point[i],2); } R=l/a/2; theta=asin(a/sqrt(l)); theta=2*theta*(1.-1./(numberOfCoronas-c+1)); if(fabs(theta)<0.001) { l=pow(a,2)/l; double fractionAngle=1.-1./(numberOfCoronas-c+1); for(int i=0; i<3; i++) { point[i]=pow(fractionAngle,2)*a*normal[i]+center[i] +fractionAngle/sqrt(1-l)*(point[i]-a*normal[i]); } } else { for(int i=0; i<3; i++) { point[i]=-R*cos(theta)*normal[i] +R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normal[i]) +center[i]+R*normal[i]; } } break; case SMOOTH_PATCH : points->GetPoint(nptsUntilCorona[c-1]+id,point); a=0; b=0; mn=0; for(int i=0; i<3; i++) { point[i]-=center[i]; a+=point[i]*normalsAtPoint[id][i]; b+=point[i]*normal[i]; mn+=normalsAtPoint[id][i]*normal[i]; } l=(mn*a-b)/(1-pow(mn,2)); signChange = (l>0); a=0; l=0; b=0; mn=0; for(int i=0; i<3; i++) { point[i]=-(point1[i]+point2[i])/2+center[i]; a+=point[i]*normalsAtPoint[id][i]; l+=pow(point[i],2); } R=l/a/2; theta=asin(a/sqrt(l)); double fractionAngle=1./(numberOfCoronas-c+1); if(signChange) fractionAngle*=-1; theta=2*theta*fractionAngle; if(fabs(theta)<0.001) { l=pow(a,2)/l; for(int i=0; i<3; i++) { point[i]=pow(fractionAngle,2)*a*normalsAtPoint[id][i] +center[i]-point[i] +fractionAngle/sqrt(1-l)*(point[i]-a*normalsAtPoint[id][i]); } } else { for(int i=0; i<3; i++) { point[i]=-R*cos(theta)*normalsAtPoint[id][i] +R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normalsAtPoint[id][i]) +center[i]-point[i]+R*normalsAtPoint[id][i]; } } for(int i=0; i<3; i++) { point1[i]-=point[i]; point2[i]-=point[i]; } for(int i=0; i<3; i++) { normalsAtPoint[nUsedIds][i]=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3]; } area=0; for(int i=0; i<3; i++) area+=pow(normalsAtPoint[nUsedIds][i],2); area=sqrt(area); for(int i=0; i<3; i++) normalsAtPoint[nUsedIds][i]/=area; break; } nUsedIds++; points->InsertNextPoint(point); } } nptsInCorona[c]=nUsedIds; onlyOneCorrespondingPoint[c].resize(nUsedIds); } nptsUntilCorona[numberOfCoronas]=nptsUntilCorona[numberOfCoronas-1]+nptsInCorona[numberOfCoronas-1]; points->InsertNextPoint(center); polys = vtkCellArray::New(); //A polygon is created for(int c=0; c<numberOfCoronas-1; c++) { int id2=0; for(int id=0; id<nptsInCorona[c] ; id++) { if(onlyOneCorrespondingPoint[c][id]) { ptIds1->Reset(); ptIds1->InsertNextId(id+nptsUntilCorona[c]); ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]); ptIds1->InsertNextId(id2%nptsInCorona[c+1]+nptsUntilCorona[c+1]); polys->InsertNextCell(ptIds1); } else { ptIds1->Reset(); ptIds1->InsertNextId(id+nptsUntilCorona[c]); ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]); ptIds1->InsertNextId(id2+nptsUntilCorona[c+1]); ptIds2->Reset(); ptIds2->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]); ptIds2->InsertNextId(id2+nptsUntilCorona[c+1]); ptIds2->InsertNextId((id2+1)%nptsInCorona[c+1]+nptsUntilCorona[c+1]); polys->InsertNextCell(ptIds1); polys->InsertNextCell(ptIds2); id2++; } } } for(int id=0; id< nptsInCorona[numberOfCoronas-1]; id++) { ptIds1->Reset(); ptIds1->InsertNextId(id+nptsUntilCorona[numberOfCoronas-1]); ptIds1->InsertNextId((id+1)%nptsInCorona[numberOfCoronas-1]+nptsUntilCorona[numberOfCoronas-1]); ptIds1->InsertNextId(nptsUntilCorona[numberOfCoronas]); polys->InsertNextCell(ptIds1); } patch = vtkPolyData::New(); patch->SetPoints(points); patch->SetPolys(polys); //Each sorted hole is stored poly[idRegion] = patch; patchOutput.at(idRegion) = vtkPolyData::New(); patchOutput.at(idRegion)->DeepCopy(patch); connec->DeleteSpecifiedRegion(idRegion); } locator->Delete(); ptIds->Delete(); ptIds1->Delete(); ptIds2->Delete(); ptIdsOrdered->Delete(); cellIds->Delete(); cellIdsOrdered->Delete(); connec->Delete(); vtkAppendPolyData *append2 = vtkAppendPolyData::New(); append2->AddInput(this->input); if(numRegions != 0) { vtkAppendPolyData *append = vtkAppendPolyData::New(); for(int idRegion = 0; idRegion < numRegions; idRegion++ ) { append->AddInput(poly[idRegion]); append->Update(); poly[idRegion]->Delete(); } append2->AddInput(append->GetOutput()); append->Delete(); } append2->Update(); vtkCleanPolyData * clean2 = vtkCleanPolyData::New(); clean2->SetInput(append2->GetOutput()); clean2->PointMergingOn(); clean2->SetTolerance(0.0); clean2->PieceInvariantOff(); clean2->Update(); append2->Delete(); normals->SetInput(clean2->GetOutput()); normals->SplittingOff(); normals->SetFeatureAngle(360.0); normals->ComputePointNormalsOff(); normals->ComputeCellNormalsOn(); normals->FlipNormalsOn(); normals->AutoOrientNormalsOn(); normals->ConsistencyOn(); normals->Update(); this->output->DeepCopy(normals->GetOutput()); normals->Delete(); clean->Delete(); clean2->Delete(); }