static void StateVectorToElements(const Point3d& position, const Vec3d& v, double GM, OrbitalElements* elements) { Vec3d R = position - Point3d(0.0, 0.0, 0.0); Vec3d L = R ^ v; double magR = R.length(); double magL = L.length(); double magV = v.length(); L *= (1.0 / magL); Vec3d W = L ^ (R / magR); // Compute the semimajor axis double a = 1.0 / (2.0 / magR - square(magV) / GM); // Compute the eccentricity double p = square(magL) / GM; double q = R * v; double ex = 1.0 - magR / a; double ey = q / sqrt(a * GM); double e = sqrt(ex * ex + ey * ey); // Compute the mean anomaly double E = atan2(ey, ex); double M = E - e * sin(E); // Compute the inclination double cosi = L * Vec3d(0, 1.0, 0); double i = 0.0; if (cosi < 1.0) i = acos(cosi); // Compute the longitude of ascending node double Om = atan2(L.x, L.z); // Compute the argument of pericenter Vec3d U = R / magR; double s_nu = (v * U) * sqrt(p / GM); double c_nu = (v * W) * sqrt(p / GM) - 1; s_nu /= e; c_nu /= e; Vec3d P = U * c_nu - W * s_nu; Vec3d Q = U * s_nu + W * c_nu; double om = atan2(P.y, Q.y); // Compute the period double T = 2 * PI * sqrt(cube(a) / GM); elements->semimajorAxis = a; elements->eccentricity = e; elements->inclination = i; elements->longAscendingNode = Om; elements->argPericenter = om; elements->meanAnomaly = M; elements->period = T; }
static EllipticalOrbit* StateVectorToOrbit(const Point3d& position, const Vec3d& v, double mass, double t) { Vec3d R = position - Point3d(0.0, 0.0, 0.0); Vec3d L = R ^ v; double magR = R.length(); double magL = L.length(); double magV = v.length(); L *= (1.0 / magL); Vec3d W = L ^ (R / magR); double G = astro::G * 1e-9; // convert from meters to kilometers double GM = G * mass; // Compute the semimajor axis double a = 1.0 / (2.0 / magR - square(magV) / GM); // Compute the eccentricity double p = square(magL) / GM; double q = R * v; double ex = 1.0 - magR / a; double ey = q / sqrt(a * GM); double e = sqrt(ex * ex + ey * ey); // Compute the mean anomaly double E = atan2(ey, ex); double M = E - e * sin(E); // Compute the inclination double cosi = L * Vec3d(0, 1.0, 0); double i = 0.0; if (cosi < 1.0) i = acos(cosi); // Compute the longitude of ascending node double Om = atan2(L.x, L.z); // Compute the argument of pericenter Vec3d U = R / magR; double s_nu = (v * U) * sqrt(p / GM); double c_nu = (v * W) * sqrt(p / GM) - 1; s_nu /= e; c_nu /= e; Vec3d P = U * c_nu - W * s_nu; Vec3d Q = U * s_nu + W * c_nu; double om = atan2(P.y, Q.y); // Compute the period double T = 2 * PI * sqrt(cube(a) / GM); T = T / 86400.0; // Convert from seconds to days return new EllipticalOrbit(a * (1 - e), e, i, Om, om, M, T, t); }
// Make a rotation Quat which will rotate vec1 to vec2 // Generally take adot product to get the angle between these // and then use a cross product to get the rotation axis // Watch out for the two special cases of when the vectors // are co-incident or opposite in direction. void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to ) { const value_type epsilon = 0.0000001; value_type length1 = from.length(); value_type length2 = to.length(); // dot product vec1*vec2 value_type cosangle = from*to/(length1*length2); if ( fabs(cosangle - 1) < epsilon ) { OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl; // cosangle is close to 1, so the vectors are close to being coincident // Need to generate an angle of zero with any vector we like // We'll choose (1,0,0) makeRotate( 0.0, 0.0, 0.0, 1.0 ); } else if ( fabs(cosangle + 1.0) < epsilon ) { // vectors are close to being opposite, so will need to find a // vector orthongonal to from to rotate about. Vec3d tmp; if (fabs(from.x())<fabs(from.y())) if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis. else tmp.set(0.0,0.0,1.0); else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0); else tmp.set(0.0,0.0,1.0); Vec3d fromd(from.x(),from.y(),from.z()); // find orthogonal axis. Vec3d axis(fromd^tmp); axis.normalize(); _v[0] = axis[0]; // sin of half angle of PI is 1.0. _v[1] = axis[1]; // sin of half angle of PI is 1.0. _v[2] = axis[2]; // sin of half angle of PI is 1.0. _v[3] = 0; // cos of half angle of PI is zero. } else { // This is the usual situation - take a cross-product of vec1 and vec2 // and that is the axis around which to rotate. Vec3d axis(from^to); value_type angle = acos( cosangle ); makeRotate( angle, axis ); } }
Vec4d Raytracer::shade(const RayIntersection& intersection, size_t depth) const { // This offset must be added to intersection points for further // traced rays to avoid noise in the image const Vec3d offset(intersection.normal() * Math::safetyEps()); Vec4d color(0,0,0,1); std::shared_ptr<const Renderable> renderable = intersection.renderable(); std::shared_ptr<const Material> material = renderable->material(); for(size_t i=0;i <mScene->lights().size();++i) { const Light &light = *(mScene->lights()[i].get()); //Shadow ray from light to hit point. const Vec3d L = (intersection.position() + offset) - light.position(); const Ray shadowRay(light.position(), L); //Shade only if light in visible from intersection point. if (!mScene->anyIntersection(shadowRay,L.length())) color += material->shade(intersection,light); } // limit recursion depth if (depth >= mMaxDepth) return color; Vec3d dir = reflect(intersection.ray().direction(), intersection.normal()); Ray reflectedRay(intersection.position() + offset, dir); double reflectance = material->reflectance(); color = color * (1 - reflectance) + reflectance * trace(reflectedRay, depth - 1) + Vec4d(0.0,0.0,0.0,1.0); return color; }
// Intersect ray r with the triangle abc. If it hits returns true, // and puts the t parameter, barycentric coordinates, normal, object id, // and object material in the isect object bool TrimeshFace::intersectLocal( const ray& r, isect& i ) const { const Vec3d& a = parent->vertices[ids[0]]; const Vec3d& b = parent->vertices[ids[1]]; const Vec3d& c = parent->vertices[ids[2]]; // tangent vectors Vec3d t1 = b - a; Vec3d t2 = c - a; Vec3d n = crossProd(t1,t2); double D = -n*a; // if the surface is parallel to the ray there is no intersection if(r.getDirection()*n == 0) { return false; } double t = -(n*r.getPosition() + D)/(n*r.getDirection() ); if (t <= RAY_EPSILON) return false; // point of intersection with the same plane (doesn't mean intersection with triangle) p(t)=p+t*d Vec3d p = r.at(t); // triangle area double A = n.length()/2.0; // barycentric coords double wa = crossProd(c-b, p-b).length() / (2.0*A); double wb = crossProd(a-c, p-c).length() / (2.0*A); double wc = crossProd(b-a, p-a).length() / (2.0*A); if((wa >= 0.0) && (wb >= 0.0) && (wc >= 0.0) && (wa+wb+wc-1.0 <= 0.00001)) { i.setT(t); i.setBary(wa, wb, wc); if (parent->normals.size() == 0) { i.setN(n); } else { Vec3d inter_n = wa*parent->normals[ids[0]] + wb*parent->normals[ids[1]] + wc*parent->normals[ids[2]]; inter_n.normalize(); i.setN(inter_n); } i.setObject(this); if (parent->materials.size() == 0) { i.setMaterial(this->getMaterial() ); } else { Material inter_m = wa*(*parent->materials[ids[0]]); inter_m += wb*(*parent->materials[ids[1]]); inter_m += wc*(*parent->materials[ids[2]]); i.setMaterial(inter_m); } return true; } return false; }
bool EclipseFinder::testEclipse(const Body& receiver, const Body& caster, double now) const { // Ignore situations where the shadow casting body is much smaller than // the receiver, as these shadows aren't likely to be relevant. Also, // ignore eclipses where the caster is not an ellipsoid, since we can't // generate correct shadows in this case. if (caster.getRadius() >= receiver.getRadius() * MinRelativeOccluderRadius && caster.isEllipsoid()) { // All of the eclipse related code assumes that both the caster // and receiver are spherical. Irregular receivers will work more // or less correctly, but casters that are sufficiently non-spherical // will produce obviously incorrect shadows. Another assumption we // make is that the distance between the caster and receiver is much // less than the distance between the sun and the receiver. This // approximation works everywhere in the solar system, and likely // works for any orbitally stable pair of objects orbiting a star. Point3d posReceiver = receiver.getAstrocentricPosition(now); Point3d posCaster = caster.getAstrocentricPosition(now); const Star* sun = receiver.getSystem()->getStar(); assert(sun != NULL); double distToSun = posReceiver.distanceFromOrigin(); float appSunRadius = (float) (sun->getRadius() / distToSun); Vec3d dir = posCaster - posReceiver; double distToCaster = dir.length() - receiver.getRadius(); float appOccluderRadius = (float) (caster.getRadius() / distToCaster); // The shadow radius is the radius of the occluder plus some additional // amount that depends upon the apparent radius of the sun. For // a sun that's distant/small and effectively a point, the shadow // radius will be the same as the radius of the occluder. float shadowRadius = (1 + appSunRadius / appOccluderRadius) * caster.getRadius(); // Test whether a shadow is cast on the receiver. We want to know // if the receiver lies within the shadow volume of the caster. Since // we're assuming that everything is a sphere and the sun is far // away relative to the caster, the shadow volume is a // cylinder capped at one end. Testing for the intersection of a // singly capped cylinder is as simple as checking the distance // from the center of the receiver to the axis of the shadow cylinder. // If the distance is less than the sum of the caster's and receiver's // radii, then we have an eclipse. float R = receiver.getRadius() + shadowRadius; double dist = distance(posReceiver, Ray3d(posCaster, posCaster - Point3d(0, 0, 0))); if (dist < R) { // Ignore "eclipses" where the caster and receiver have // intersecting bounding spheres. if (distToCaster > caster.getRadius()) return true; } } return false; }
double PointLight::distanceAttenuation( const Vec3d& P ) const { // YOUR CODE HERE // These three values are the a, b, and c in the distance // attenuation function (from the slide labelled // "Intensity drop-off with distance"): // f(d) = min( 1, 1/( a + b d + c d^2 ) ) // float constantTerm; // a // float linearTerm; // b // float quadraticTerm; // c // You'll need to modify this method to attenuate the intensity // of the light based on the distance between the source and the // point P. For now, we assume no attenuation and just return 1.0 Vec3d distance = position - P; double d = distance.length(); double intensity = min(1.0, 1.0/(constantTerm + linearTerm*d + quadraticTerm*pow(d,2))); return intensity; }
void VRConstraint::setRConstraint(Vec3d params, TCMode mode, bool local) { if (params.length() > 1e-4 && mode != POINT) params.normalize(); this->local = local; active = true; if (mode == POINT) { setMinMax(3, params[0], params[0]); setMinMax(4, params[1], params[1]); setMinMax(5, params[2], params[2]); } if (mode == LINE) { auto p = Vec3d(refMatrixA[3]); lock({3,4}); free({5}); auto po = Pose::create(p, params); po->makeUpOrthogonal(); setReferenceA( po ); } if (mode == PLANE) { auto p = Vec3d(refMatrixA[3]); lock({4}); free({3,5}); auto po = Pose::create(p, Vec3d(refMatrixA[2]), params); po->makeDirOrthogonal(); setReferenceA( po ); } }
// Apply the Blinn-Phong model to this point on the surface of the object, // returning the color of that point. Vec3d Material::shade( Scene *scene, const ray& r, const isect& i ) const { // YOUR CODE HERE // For now, this method just returns the diffuse color of the object. // This gives a single matte color for every distinct surface in the // scene, and that's it. Simple, but enough to get you started. // (It's also inconsistent with the Phong model...) // Your mission is to fill in this method with the rest of the phong // shading model, including the contributions of all the light sources. // You will need to call both distanceAttenuation() and shadowAttenuation() // somewhere in your code in order to compute shadows and light falloff. if (debugMode) std::cout << "Debugging the Phong code (or lack thereof...)" << std::endl; // When you're iterating through the lights, // you'll want to use code that looks something // like this: Vec3d light = ke(i); Vec3d normal = i.N; Vec3d iDot = r.at(i.t); if (r.getDirection() * normal > 0) { normal = -normal; light += prod(prod(scene->ambient(), ka(i)), kt(i)); } else { light += prod(scene->ambient(), ka(i)); } for (vector<Light*>::const_iterator litr = scene->beginLights(); litr != scene->endLights(); ++litr) { Light* pLight = *litr; double distAttenuation = pLight->distanceAttenuation(iDot); Vec3d shadowAttenuation = pLight->shadowAttenuation(iDot); Vec3d atten = distAttenuation * shadowAttenuation; Vec3d L = pLight->getDirection(iDot); if (L * normal > 0) { Vec3d H = (L + -1 * r.getDirection()); if (H.length() != 0) H.normalize(); double sDot = max(0.0, normal * H); Vec3d dTerm = kd(i) * (normal * L); Vec3d sTerm = ks(i) * (pow(sDot, shininess(i))); Vec3d newLight = dTerm + sTerm; newLight = prod(newLight, pLight->getColor()); light += prod(atten, newLight); } } return light; }
Vec3d rectToSpherical(const Vec3d& v) { double r = v.length(); double theta = atan2(v.y, v.x); if (theta < 0) theta = theta + 2 * PI; double phi = asin(v.z / r); return Vec3d(theta, phi, r); }
static int vector_length(lua_State* l) { CelxLua celx(l); celx.checkArgs(1, 1, "No arguments expected for vector:length"); Vec3d* v = this_vector(l); double length = v->length(); lua_pushnumber(l, (lua_Number)length); return 1; }
Vec3d PointLight::shadowAttenuation(const Vec3d& P) const { // YOUR CODE HERE: // You should implement shadow-handling code here. Vec3d direction = getDirection(P); ray r(P, direction, ray::SHADOW ); isect i; if(scene->intersect( r, i )){ Vec3d iposition = r.at(i.t); Vec3d iray = iposition - P; Vec3d lightray = position - P; double dlight = lightray.length(); double diray = iray.length(); if(diray > dlight) return Vec3d(1,1,1); else return i.getMaterial().kt(i); } else { return Vec3d(1,1,1); } }
void VRAdjacencyGraph::compCurvatures(int range) { vertex_curvatures.clear(); auto sgeo = geo.lock(); if (!sgeo) return; auto pos = sgeo->getMesh()->geo->getPositions(); auto norms = sgeo->getMesh()->geo->getNormals(); int N = pos->size(); /*auto curvMax = [&](int i, int range) { Vec3d n = norms->getValue<Vec3f>(i); Vec3d vi = pos->getValue<Pnt3f>(i); float K = 0; float Kmax = 0; auto Ne = getNeighbors(i,range); if (Ne.size() == 0) return K; for (int j : Ne) { if (j >= N) continue; Vec3d d = pos->getValue<Pnt3f>(j) - vi; float k = 2*n.dot(d)/d.squareLength(); if (abs(k) > Kmax) { K = k; Kmax = abs(k); } } return K; };*/ auto curvAvg = [&](int i, int range) { Vec3d n = Vec3d(norms->getValue<Vec3f>(i)); Pnt3f vi = pos->getValue<Pnt3f>(i); float K = 0; auto Ne = getNeighbors(i,range); if (Ne.size() == 0) return K; for (int j : Ne) { if (j >= N) continue; Vec3d d = Vec3d(pos->getValue<Pnt3f>(j) - vi); K += 2*n.dot(d)/d.length(); } K /= Ne.size(); return K; }; vertex_curvatures.resize(N); for (int i = 0; i < N; i++) vertex_curvatures[i] = curvAvg(i,range); }
void TerrainManipulator::clampOrientation() { if (!getVerticalAxisFixed()) { Matrixd rotation_matrix; rotation_matrix.makeRotate(_rotation); Vec3d lookVector = -getUpVector(rotation_matrix); Vec3d upVector = getFrontVector(rotation_matrix); CoordinateFrame coordinateFrame = getCoordinateFrame(_center); Vec3d localUp = getUpVector(coordinateFrame); //Vec3d localUp = _previousUp; Vec3d sideVector = lookVector ^ localUp; if (sideVector.length()<0.1) { OSG_INFO<<"Side vector short "<<sideVector.length()<<std::endl; sideVector = upVector^localUp; sideVector.normalize(); } Vec3d newUpVector = sideVector^lookVector; newUpVector.normalize(); Quat rotate_roll; rotate_roll.makeRotate(upVector,newUpVector); if (!rotate_roll.zeroRotation()) { _rotation = _rotation * rotate_roll; } } }
void VRCamera::focusObject(VRObjectPtr t) { auto bb = t->getBoundingbox(); Vec3d c = bb->center(); Vec3d d = getDir(); focusPoint(c); Vec3d dp = getDir(); if (dp.length() > 1e-4) d = dp; // only use new dir if it is valid d.normalize(); //float r = max(bb->radius()*2, 0.1f); float r = bb->radius() / tan(fov*0.5); setFrom(c - d*r); // go back or forth to see whole node //cout << "VRCamera::focus " << t->getName() << " pos " << c << " size " << r << endl; }
//-------------------------------------------------------------------------------------------------- /// Repositions and orients the camera to view the rotation point along the /// direction "alongDirection". The distance to the rotation point is maintained. /// //-------------------------------------------------------------------------------------------------- void ManipulatorTrackball::setView( const Vec3d& alongDirection, const Vec3d& upDirection ) { if (m_camera.isNull()) return; Vec3d dir = alongDirection; if (!dir.normalize()) return; Vec3d up = upDirection; if(!up.normalize()) up = Vec3d::Z_AXIS; if((up * dir) < 1e-2) up = dir.perpendicularVector(); Vec3d cToE = m_camera->position() - m_rotationPoint; Vec3d newEye = m_rotationPoint - cToE.length() * dir; m_camera->setFromLookAt(newEye, m_rotationPoint, upDirection); }
void interpolator::evalVec(GeoVectorProperty* pvec, int power, GeoVectorProperty* cvec, float cscale, float dl_max) { Vec3d* data = (Vec3d*)pvec->editData(); Vec4d* cdata = 0; if (cvec) cdata = (Vec4d*)cvec->editData(); float eps = 1e-5; float dl; for (uint i=0; i<pvec->size(); i++) { Vec3d d = eval(data[i], power); data[i] += d; if (cdata) { dl = d.length(); float l = dl / max(cscale*dl_max, eps); cdata[i] = Vec4d(l, 1-l, 0, 1); } } }
// going from observed position to geometrical position. void Refraction::innerRefractionBackward(Vec3d& altAzPos) const { const double length = altAzPos.length(); if (length==0.0) { // Under some circumstances there are zero coordinates. Just leave them alone. //qDebug() << "Refraction::innerRefractionBackward(): Zero vector detected - Continue with zero vector."; return; } Q_ASSERT(length>0.0); const double sinObs = altAzPos[2]/length; Q_ASSERT(fabs(sinObs)<=1.0); float obs_alt_deg=180./M_PI*std::asin(sinObs); if (obs_alt_deg > 0.22879f) { // refraction from Bennett, in Meeus, Astr.Alg. float r=press_temp_corr * (1.f / std::tan((obs_alt_deg+7.31f/(obs_alt_deg+4.4f))*M_PI/180.f) + 0.0013515f); obs_alt_deg -= r; } else if (obs_alt_deg > MIN_APP_ALTITUDE_DEG) { // backward refraction from polynomial fit against Saemundson[-5...-0.3] float r=(((((0.0444f*obs_alt_deg+.7662f)*obs_alt_deg+4.9746f)*obs_alt_deg+13.599f)*obs_alt_deg+8.052f)*obs_alt_deg-11.308f)*obs_alt_deg+34.341f; obs_alt_deg -= press_temp_corr*r; } else if (obs_alt_deg > MIN_APP_ALTITUDE_DEG-TRANSITION_WIDTH_APP_DEG) { // Compute top value from polynome, apply linear interpolation static const float r_min=(((((0.0444f*MIN_APP_ALTITUDE_DEG+.7662f)*MIN_APP_ALTITUDE_DEG +4.9746f)*MIN_APP_ALTITUDE_DEG+13.599f)*MIN_APP_ALTITUDE_DEG +8.052f)*MIN_APP_ALTITUDE_DEG-11.308f)*MIN_APP_ALTITUDE_DEG+34.341f; obs_alt_deg -= r_min*press_temp_corr*(obs_alt_deg-(MIN_APP_ALTITUDE_DEG-TRANSITION_WIDTH_APP_DEG))/TRANSITION_WIDTH_APP_DEG; } else return; // At this point we have corrected observed altitude. Note that if we just change altAzPos[2], we would change vector length, so this would change our angles. // We have to make X,Y components of the vector a bit longer as well by the change in cosines of altitude, or (sqrt(1-sin(alt)) const double geo_alt_rad=obs_alt_deg*M_PI/180.; const double sinGeo=std::sin(geo_alt_rad); const double longerxy=((fabs(sinObs)>=1.0) ? 1.0 : std::sqrt((1.-sinGeo*sinGeo)/(1.-sinObs*sinObs))); altAzPos[0]*=longerxy; altAzPos[1]*=longerxy; altAzPos[2]=sinGeo*length; }
bool Geometry::intersect(const ray&r, isect&i) const { double tmin, tmax; if (hasBoundingBoxCapability() && !(bounds.intersect(r, tmin, tmax))) return false; // Transform the ray into the object's local coordinate space Vec3d pos = transform->globalToLocalCoords(r.getPosition()); Vec3d dir = transform->globalToLocalCoords(r.getPosition() + r.getDirection()) - pos; double length = dir.length(); dir /= length; ray localRay( pos, dir, r.type() ); if (intersectLocal(localRay, i)) { // Transform the intersection point & normal returned back into global space. i.N = transform->localToGlobalCoordsNormal(i.N); i.t /= length; return true; } else return false; }
//-------------------------------------------------------------------------------------------------- /// Project the given vector onto the plane /// /// \param vector Vector to be projected /// \param projectedVector Projected vector to be returned by pointer /// /// \return true if successfully projected. /// false if the given \a vector is parallel with the plane's normal //-------------------------------------------------------------------------------------------------- bool Plane::projectVector(const Vec3d& vector, Vec3d* projectedVector) const { CVF_ASSERT(projectedVector); Vec3d n = normal(); Vec3d tmp = n ^ vector; Vec3d k = tmp ^ n; double length = k.length(); if (length <= 0.0) return false; k *= 1.0/length; length = vector*k; *projectedVector = k; *projectedVector *= length; return true; }
static int position_distanceto(lua_State* l) { CelxLua celx(l); celx.checkArgs(2, 2, "One argument expected to position:distanceto()"); UniversalCoord* uc = this_position(l); UniversalCoord* uc2 = to_position(l, 2); if (uc2 == NULL) { celx.doError("Position expected as argument to position:distanceto"); } Vec3d v = *uc2 - *uc; lua_pushnumber(l, astro::microLightYearsToKilometers(v.length())); return 1; }
//-------------------------------------------------------------------------------------------------- /// Compute quaternion rotation /// /// \param oldPosX x coordinate of the last position of the mouse, in the range [-1.0, 1.0] /// \param oldPosY y coordinate of the last position of the mouse, in the range [-1.0, 1.0] /// \param newPosX x coordinate of current position of the mouse, in the range [-1.0, 1.0] /// \param newPosY y coordinate of current position of the mouse, in the range [-1.0, 1.0] /// \param currViewMatrix Current transformation matrix. The inverse is used when calculating the rotation /// \param sensitivityFactor Mouse sensitivity factor /// /// Simulate a track-ball. Project the points onto the virtual trackball, then figure out the axis /// of rotation. This is a deformed trackball-- is a trackball in the center, but is deformed into a /// hyperbolic sheet of rotation away from the center. //-------------------------------------------------------------------------------------------------- Quatd ManipulatorTrackball::trackballRotation(double oldPosX, double oldPosY, double newPosX, double newPosY, const Mat4d& currViewMatrix, double sensitivityFactor) { // This particular function was chosen after trying out several variations. // Implemented by Gavin Bell, lots of ideas from Thant Tessman and the August '88 // issue of Siggraph's "Computer Graphics," pp. 121-129. // This size should really be based on the distance from the center of rotation to the point on // the object underneath the mouse. That point would then track the mouse as closely as possible. const double TRACKBALL_RADIUS = 0.8f; // Clamp to valid range oldPosX = Math::clamp(oldPosX, -1.0, 1.0); oldPosY = Math::clamp(oldPosY, -1.0, 1.0); newPosX = Math::clamp(newPosX, -1.0, 1.0); newPosY = Math::clamp(newPosY, -1.0, 1.0); // First, figure out z-coordinates for projection of P1 and P2 to deformed sphere Vec3d p1 = projectToSphere(TRACKBALL_RADIUS, oldPosX, oldPosY); Vec3d p2 = projectToSphere(TRACKBALL_RADIUS, newPosX, newPosY); // Axis of rotation is the cross product of P1 and P2 Vec3d a = p1 ^ p2; // Figure out how much to rotate around that axis. Vec3d d = p1 - p2; double t = d.length()/(2.0*TRACKBALL_RADIUS); // Avoid problems with out-of-control values... t = Math::clamp(t, -1.0, 1.0); double phi = 2.0*asin(t); // Scale by sensitivity factor phi *= sensitivityFactor; // Use inverted matrix to find rotation axis Mat4d invMatr = currViewMatrix.getInverted(); a.transformVector(invMatr); // Get quaternion to be returned by pointer Quatd quat = Quatd::fromAxisAngle(a, phi); return quat; }
/* * Check if the point could be an obstacle to a forward movement. * The route is given as a pair of source and destination. * The argument `dis` is used as the threashold of distance * between the route and the point. */ bool SampleBase::checkConflict(const Vec3d& src,const Vec3d& dest,const Vec3d& point,double dis) { Vec3d dir; dir = dest - src; double dirLength = dir.length(); if (dirLength!=0.0) dir = (1.0/dirLength) * dir; Vec3d pDir; pDir = point - src; if ((pDir * dir)<0.0) return false; if ((pDir * dir)>dirLength) return false; Vec3d dir2 = Vec3d(dir); dir2 = dir2.simpleRotateY(90); if (fabs(pDir * dir2) < dis) return true; else return false; }
void Refraction::innerRefractionForward(Vec3d& altAzPos) const { const double length = altAzPos.length(); if (length==0.0) { // Under some circumstances there are zero coordinates. Just leave them alone. //qDebug() << "Refraction::innerRefractionForward(): Zero vector detected - Continue with zero vector."; return; } Q_ASSERT(length>0.0); const double sinGeo = altAzPos[2]/length; Q_ASSERT(fabs(sinGeo)<=1.0); double geom_alt_rad = std::asin(sinGeo); float geom_alt_deg = 180./M_PI*geom_alt_rad; if (geom_alt_deg > MIN_GEO_ALTITUDE_DEG) { // refraction from Saemundsson, S&T1986 p70 / in Meeus, Astr.Alg. float r=press_temp_corr * ( 1.02f / std::tan((geom_alt_deg+10.3f/(geom_alt_deg+5.11f))*M_PI/180.f) + 0.0019279f); geom_alt_deg += r; if (geom_alt_deg > 90.f) geom_alt_deg=90.f; } else if(geom_alt_deg>MIN_GEO_ALTITUDE_DEG-TRANSITION_WIDTH_GEO_DEG) { // Avoids the jump below -5 by interpolating linearly between MIN_GEO_ALTITUDE_DEG and bottom of transition zone float r_m5=press_temp_corr * ( 1.02f / std::tan((MIN_GEO_ALTITUDE_DEG+10.3f/(MIN_GEO_ALTITUDE_DEG+5.11f))*M_PI/180.f) + 0.0019279f); geom_alt_deg += r_m5*(geom_alt_deg-(MIN_GEO_ALTITUDE_DEG-TRANSITION_WIDTH_GEO_DEG))/TRANSITION_WIDTH_GEO_DEG; } else return; // At this point we have corrected geometric altitude. Note that if we just change altAzPos[2], we would change vector length, so this would change our angles. // We have to shorten X,Y components of the vector as well by the change in cosines of altitude, or (sqrt(1-sin(alt)) const double refr_alt_rad=geom_alt_deg*M_PI/180.; const double sinRef=std::sin(refr_alt_rad); const double shortenxy=((fabs(sinGeo)>=1.0) ? 1.0 : std::sqrt((1.-sinRef*sinRef)/(1.-sinGeo*sinGeo))); // we need double's mantissa length here, sorry! altAzPos[0]*=shortenxy; altAzPos[1]*=shortenxy; altAzPos[2]=sinRef*length; }
void scene::addText(const std::string & l, unsigned short color, Vec3d & point, osgText::Text *text) { dxfLayer* layer = _layerTable->findOrCreateLayer(l); if (layer->getFrozen()) return; sceneLayer* ly = findOrCreateSceneLayer(l); // Apply the scene settings to the text size and rotation Vec3d pscene(addVertex(point)); Vec3d xvec = addVertex( point + (text->getRotation() * X_AXIS) ) - pscene; Vec3d yvec = addVertex( point + (text->getRotation() * Y_AXIS) ) - pscene; text->setCharacterSize( text->getCharacterHeight()*yvec.length(), text->getCharacterAspectRatio()*yvec.length()/xvec.length() ); Matrix qm = _r * _m; Vec3d t, s; Quat ro, so; qm.decompose( t, ro, s, so ); text->setRotation( text->getRotation() * ro ); sceneLayer::textInfo ti( correctedColorIndex(l,color), pscene, text ); ly->_textList.push_back(ti); }
Vec4d Raytracer::shade(const RayIntersection &intersection, size_t depth) const { // This offset must be added to intersection points for further // traced rays to avoid noise in the image const Vec3d offset(intersection.normal() * Math::safetyEps()); Vec4d color(0, 0, 0, 1); std::shared_ptr<const Renderable> renderable = intersection.renderable(); std::shared_ptr<const Material> material = renderable->material(); for (size_t i = 0; i < mScene->lights().size(); ++i) { const Light &light = *(mScene->lights()[i].get()); //Shadow ray from light to hit point. const Vec3d L = (intersection.position() + offset) - light.position(); const Ray shadowRay(light.position(), L); //Shade only if light in visible from intersection point. if (!mScene->anyIntersection(shadowRay, L.length())) color += material->shade(intersection, light); } return color; }
bool RayIntersector::intersects(const BoundingSphere& bs) { // if bs not valid then return false based on the assumption that the node is empty. if (!bs.valid()) return false; // test for _start inside the bounding sphere Vec3d sm = _start - bs._center; double c = sm.length2() - bs._radius * bs._radius; if (c<0.0) return true; // solve quadratic equation double a = _direction.length2(); double b = (sm * _direction) * 2.0; double d = b * b - 4.0 * a * c; // no intersections if d<0 if (d<0.0) return false; // compute two solutions of quadratic equation d = sqrt(d); double div = 1.0/(2.0*a); double r1 = (-b-d)*div; double r2 = (-b+d)*div; // return false if both intersections are before the ray start if (r1<=0.0 && r2<=0.0) return false; // if LIMIT_NEAREST and closest point of bounding sphere is further than already found intersection, return false if (_intersectionLimit == LIMIT_NEAREST && !getIntersections().empty()) { double minDistance = sm.length() - bs._radius; if (minDistance >= getIntersections().begin()->distance) return false; } // passed all the rejection tests so line must intersect bounding sphere, return true. return true; }
/* * Back this car to the given location (v). */ void SampleBase::backToDestination(const Vec3d& v) { double power = 0.0; double steering = 0.0; Vec3d dir; dir = v - loc; double dis = dir.length(); if (dis!=0.0) dir = (1.0/dis) * dir; double targetVel = dis > 15 ? 15 : dis; if ((dir * front) < -0.7) { steering = 0.3 * (dir * left); power = -300*(targetVel - vel.length()); power = power > 300 ? 300 : power; power = power < -300 ? -300 : power; } else { steering = 1.0; power = 150; } ostringstream oss; oss << "drive " << setprecision(16) << power << " " << steering; socket->send(oss.str().c_str()); }
// Turn void ViroManipulator::turn(double dx, double dy){ double dt = getDtime(); _bTumble = false; //_bTurning = false; if (!_bHoldCTRL){ //_bTurning = true; double dPitch = -inDegrees(dy * PeripheralSensibility * dt); double dYaw = inDegrees(dx * PeripheralSensibility * dt); double dRoll = dYaw * RollWithYaw; // Update global pitch & yaw. UNUSED _pitch += dPitch; _yaw += dYaw; osg::Quat dRotation; // Delta Rotation osg::Quat pitch_rotate; osg::Quat yaw_rotate; pitch_rotate.makeRotate(dPitch, _vStrafe); yaw_rotate.makeRotate(dYaw, _vUp); osg::Quat roll_rotate; if (RollWithYaw != 0.0){ roll_rotate.makeRotate(-dRoll, _vLook); dRotation = pitch_rotate * roll_rotate * yaw_rotate; } else dRotation = pitch_rotate * yaw_rotate; // Apply deltaRotation and store it. _qRotation = _qRotation * dRotation; } else { long double Fy = (dt * 200 * dy); long double Fx = (dt * 200 * dx); if (isEnabled(AUTO_SCALE_STEP)){ Fy *= autoStepFactor; Fx *= autoStepFactor; } osg::Quat qTumble; //dRotation.makeRotate(-(Fx * (_vEye-_vLastPickedPoint).length() * 0.1 ), osg::Z_AXIS); qTumble = computeQuat(_vEye,_vLastPickedPoint, osg::Z_AXIS); Vec3d M,Slide; Slide = _vStrafe; Slide.normalize(); Slide *= -Fx; //double rx,ry,X,Y,a; //a = osg::DegreesToRadians( VecAngle(osg::X_AXIS, _vLook) ); /* X = _vLastPickedPoint.x() - _vEye.x(); Y = _vLastPickedPoint.y() - _vEye.y(); rx = X*cos(a) - Y*sin(a); ry = X*sin(a) + Y*cos(a); */ Vec3d V1 = _vLastPickedPoint - _vEye; double rad = V1.length(); Vec3d V2 = _vLook; V1.normalize(); V2.normalize(); Vec3d T = (Vec3d(_vEye+V1) - Vec3d(_vEye+V2)); //double a = 0.1; //rx = X*cos(a) - Y*sin(a); //ry = X*sin(a) + Y*cos(a); #define VIRO_TUMBLESNAPSENSIBILITY 0.1 if (T.length() < VIRO_TUMBLESNAPSENSIBILITY){ _bTumble = true; osg::Vec3d tmp; _qRotation = qTumble; //M = Vec3d(X,Y,-Fy); M = Vec3d(Slide.x()*2,Slide.y()*2,-Fy); tmp = _vEye + M; tmp = _vLastPickedPoint-tmp; tmp.normalize(); tmp *= rad; _vEye = _vLastPickedPoint - tmp; //_vEye += M; //_vEye = _vLastPickedPoint - M; } else { M = Vec3d(Slide.x(),Slide.y(),-Fy); _vTarget += M; _vEye += M; } } }
void Meteor::init(const float& radiantAlpha, const float& radiantDelta, const float& speed, const QList<ColorPair> colors) { // meteor velocity in km/s m_speed = speed; // find the radiant in horizontal coordinates Vec3d radiantAltAz; StelUtils::spheToRect(radiantAlpha, radiantDelta, radiantAltAz); radiantAltAz = m_core->j2000ToAltAz(radiantAltAz); float radiantAlt, radiantAz; // S is zero, E is 90 degrees (SDSS) StelUtils::rectToSphe(&radiantAz, &radiantAlt, radiantAltAz); // meteors won't be visible if radiant is below 0degrees if (radiantAlt < 0.f) { return; } // define the radiant coordinate system // rotation matrix to align z axis with radiant m_matAltAzToRadiant = Mat4d::zrotation(radiantAz) * Mat4d::yrotation(M_PI_2 - radiantAlt); // select a random initial meteor altitude in the horizontal system [MIN_ALTITUDE, MAX_ALTITUDE] float initialAlt = MIN_ALTITUDE + (MAX_ALTITUDE - MIN_ALTITUDE) * ((float) qrand() / ((float) RAND_MAX + 1)); // calculates the max z-coordinate for the currrent radiant float maxZ = meteorZ(M_PI_2 - radiantAlt, initialAlt); // meteor trajectory // select a random xy position in polar coordinates (radiant system) float xyDist = maxZ * ((double) qrand() / ((double) RAND_MAX + 1)); // [0, maxZ] float theta = 2 * M_PI * ((double) qrand() / ((double) RAND_MAX + 1)); // [0, 2pi] // initial meteor coordinates (radiant system) m_position[0] = xyDist * qCos(theta); m_position[1] = xyDist * qSin(theta); m_position[2] = maxZ; m_posTrain = m_position; // store the initial z-component (radiant system) m_initialZ = m_position[2]; // find the initial meteor coordinates in the horizontal system Vec3d positionAltAz = m_position; positionAltAz.transfo4d(m_matAltAzToRadiant); // find the angle from horizon to meteor float meteorAlt = qAsin(positionAltAz[2] / positionAltAz.length()); // this meteor should not be visible if it is above the maximum altitude // or if it's below the horizon! if (positionAltAz[2] > MAX_ALTITUDE || meteorAlt <= 0.f) { return; } // determine the final z-component and the min distance between meteor and observer if (radiantAlt < 0.0262f) // (<1.5 degrees) earth grazing meteor ? { // earth-grazers are rare! // introduce a probabilistic factor just to make them a bit harder to occur float prob = ((float) qrand() / ((float) RAND_MAX + 1)); if (prob > 0.3f) { return; } // limit lifetime to 12sec m_finalZ = -m_position[2]; m_finalZ = qMax(m_position[2] - m_speed * 12.f, (double) m_finalZ); m_minDist = xyDist; } else { // limit lifetime to 12sec m_finalZ = meteorZ(M_PI_2 - meteorAlt, MIN_ALTITUDE); m_finalZ = qMax(m_position[2] - m_speed * 12.f, (double) m_finalZ); m_minDist = qSqrt(m_finalZ * m_finalZ + xyDist * xyDist); } // a meteor cannot hit the observer! if (m_minDist < MIN_ALTITUDE) { return; } // select random magnitude [-3; 4.5] float Mag = (float) qrand() / ((float) RAND_MAX + 1) * 7.5f - 3.f; // compute RMag and CMag RCMag rcMag; m_core->getSkyDrawer()->computeRCMag(Mag, &rcMag); m_absMag = rcMag.radius <= 1.2f ? 0.f : rcMag.luminance; if (m_absMag == 0.f) { return; } // most visible meteors are under about 184km distant // scale max mag down if outside this range float scale = qPow(184.0 / m_minDist, 2); m_absMag *= qMin(scale, 1.0f); // build the color vector buildColorVectors(colors); m_alive = true; }