// 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 ); } }
void AngleMeasure::calculateEndsOneLine(const Vec3d start, const Vec3d end, Vec3d &perp1Start, Vec3d &perp1End, Vec3d &perp2Start, Vec3d &perp2End, double &angle) { Vec3d v0 = end - start; Vec3d v1 = Vec3d(0,0,0) - start; Vec3d p = v0 ^ v1; p *= 0.08; // end width perp1Start.set(start[0]-p[0],start[1]-p[1],start[2]-p[2]); perp1End.set(start[0]+p[0],start[1]+p[1],start[2]+p[2]); v1 = Vec3d(0,0,0) - end; p = v0 ^ v1; p *= 0.08; // end width perp2Start.set(end[0]-p[0],end[1]-p[1],end[2]-p[2]); perp2End.set(end[0]+p[0],end[1]+p[1],end[2]+p[2]); angle = start.angle(end); }
// sampleGridArroundAtoms // takes from a 3D grid values at grid points which are in area between $atom_Rmin[i] to $atom_Rmaxs[i] distance from any i-th atom at position $atom_posp[i] // if $canStore == true it will save values to $sampled_val and postions to $sampled_pos else it only returns number of gridpoints fullfilling the conditions int sampleGridArroundAtoms( int natoms, Vec3d * atom_pos_, double * atom_Rmin, double * atom_Rmax, bool * atom_mask, double * sampled_val, Vec3d * sampled_pos_, bool canStore, bool pbc, bool show_where ){ Vec3d * atom_pos = (Vec3d*) atom_pos_; Vec3d * sampled_pos = (Vec3d*) sampled_pos_; int nx = GRID::n.x; int ny = GRID::n.y; int nz = GRID::n.z; int nxy = ny * nx; // used in macro i3D( ia, ib, ic ) Vec3d rProbe; rProbe.set( 0.0, 0.0, 0.0 ); // we may shift here int points_found = 0; int nimg = 1; if (pbc) nimg = 27; // PBC ? for ( int ia=0; ia<nx; ia++ ){ //printf( " ia %i \n", ia ); rProbe.add( GRID::dCell.a ); for ( int ib=0; ib<ny; ib++ ){ rProbe.add( GRID::dCell.b ); for ( int ic=0; ic<nz; ic++ ){ rProbe.add( GRID::dCell.c ); bool withinRmin = false; bool withinRmax = false; for ( int iimg=0; iimg<nimg; iimg++ ){ Vec3d cell_shift; cell_shift.set_lincomb( images[iimg][0], images[iimg][1], images[iimg][2], GRID::cell.a, GRID::cell.b, GRID::cell.c ); for ( int iatom=0; iatom<natoms; iatom++ ){ Vec3d dr; dr.set_sub( rProbe, atom_pos[iatom] ); dr.sub( cell_shift ); double r2 = dr.norm2(); double rmin = atom_Rmin[iatom]; double rmax = atom_Rmax[iatom]; if( r2<(rmax*rmax) ){ if( atom_mask[iatom] ){ withinRmax = true; } if( r2<(rmin*rmin) ){ withinRmin = true; break; } } } } if( withinRmax && (!withinRmin) ){ if( canStore ){ sampled_val[points_found] = GRID::grid[ i3D( ia, ib, ic ) ]; if( show_where ) GRID::grid[ i3D( ia, ib, ic ) ] = +100.0d; sampled_pos[points_found].set( rProbe ); } points_found++; } } rProbe.add_mul( GRID::dCell.c, -nz ); } rProbe.add_mul( GRID::dCell.b, -ny ); } return points_found; }
// FUNCTION ====== draw void draw(){ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); Vec3d oldPos; oldPos.set( body.pos ); //printVec( oldPos ); printf(" // oldPos \n"); /* printf("invIbody\n"); printMat(rbody.invIbody); printf("invI\n"); printMat(rbody.invI); printf("rotMat\n"); printMat(rbody.rotMat); printQuat(rbody.qrot ); printf("qrot\n"); printVec(rbody.L ); printf("L\n"); printVec(rbody.omega); printf("omega\n"); printVec(rbody.pos ); printf("pos\n"); */ physics(); // ALL PHYSICS COMPUTATION DONE HERE camera (); //drawLine( oldPos, body.pos ); glEnable (GL_LIGHTING); glShadeModel(GL_FLAT); rbody.render(); rbody2.render(); glColor3f( 0, 0, 0); constrain1->render(); /* printf("invI\n"); printMat(rbody.invI); printf("rotMat\n"); printMat(rbody.rotMat); printQuat(rbody.qrot ); printf("qrot\n"); printVec(rbody.L ); printf("L\n"); printVec(rbody.omega); printf("omega\n"); printVec(rbody.pos ); printf("pos\n"); quit(); */ drawAxis( 5 ); glFinish(); SDL_GL_SwapBuffers(); }
void setupBlockWorld( const Vec3d& pos0_, const Vec3d& scaling_, const Mat3d& rot ){ pos0 .set( pos0_ ); scaling.set( scaling_ ); buildRotations( rot ); /* printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos0.x, pos0.y, pos0.z ); //printf( " scaling_ (%3.3f,%3.3f,%3.3f) \n", scaling_.x, scaling_.y, scaling_.z ); printf( " scaling (%3.3f,%3.3f,%3.3f) \n", scaling.x, scaling.y, scaling.z ); printf( " a (%3.3f,%3.3f,%3.3f) \n", rotations[0].a.x, rotations[0].a.y, rotations[0].a.z ); printf( " b (%3.3f,%3.3f,%3.3f) \n", rotations[0].b.x, rotations[0].b.y, rotations[0].b.z ); printf( " c (%3.3f,%3.3f,%3.3f) \n", rotations[0].c.x, rotations[0].c.y, rotations[0].c.z ); */ }
void setup(){ int ifree,igl,nvert,ndiv; ray0.set( 1.0, 1.0, 1.0 ); hRay.set( -1.0, -1.0, -1.0 ); hRay.normalize(); a.set( 1.0, 0.0, 0.0 ); b.set( 0.0, 1.0, 0.0 ); c.set( 0.0, 0.0, 1.0 ); bool inside = false; double dist = rayTriangle( ray0, hRay, a,b,c, inside, phit ); if(inside){ printf( " triagnle hit: TRUE in dist: %f ", inside, dist ); }else{ printf( " triagnle hit: FALSE in dist: %f ", inside, dist ); } ifree = glObjects->getFree(); igl = glGenLists(1); glObjects->data[ ifree ] = igl; glNewList( igl , GL_COMPILE ); glColor3f( 0.9,0.9,0.9 ); drawPointCross( a, 0.1 ); drawPointCross( b, 0.1 ); drawPointCross( c, 0.1 ); drawTriangle( a, b, c ); glColor3f( 0.1,0.1,0.1 ); drawVecInPos ( hRay*10, ray0 ); drawPointCross( phit, 0.2 ); glEndList(); }
//-------------------------------------------------------------------------------------------------- /// Transform the min and max coordinate with the given transformation matrix //-------------------------------------------------------------------------------------------------- void BoundingBox::transform(const Mat4d& matrix) { // Check if box is invalid, and don't transform if so if (!isValid()) return; BoundingBox newBox; newBox.reset(); Vec3d node; node.set(m_min.x(), m_min.y(), m_min.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_max.x(), m_min.y(), m_min.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_max.x(), m_max.y(), m_min.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_min.x(), m_max.y(), m_min.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_min.x(), m_min.y(), m_max.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_max.x(), m_min.y(), m_max.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_max.x(), m_max.y(), m_max.z()); node.transformPoint(matrix); newBox.add(node); node.set(m_min.x(), m_max.y(), m_max.z()); node.transformPoint(matrix); newBox.add(node); *this = newBox; }
void drawForces( double scale, bool DEBUG ){ if( ( world.truss.points != NULL )&&( world.truss.forces != NULL ) ){ glBegin( GL_LINES ); for( int i=0; i<world.truss.npoints; i++ ){ Vec3d& p = world.truss.points[i]; Vec3d& f = world.truss.forces[i]; Vec3d pf; pf.set(p); pf.add_mul( f, scale ); glVertex3f( (float)p.x, (float)p.y, (float)p.z ); glVertex3f( (float)pf.x, (float)pf.y, (float)pf.z ); if( DEBUG ){ //printf( " %i %i %i (%3.3f,%3.3f,%3.3f) (%3.3f,%3.3f,%3.3f)\n", i, bond.i, bond.j, pi.x, pi.y, pi.z, f.x, f.y, f.z ); printf( " %i (%3.3f,%3.3f,%3.3f) \n", i, f.x, f.y, f.z ); } } glEnd(); } }
bool CEvent_UpdatePositionParametricExpression::Update(CEnvironment* p_env) { float x,y,z; m_expression_variables_x.mv_variable[0].m_value= p_env->get_elapsed_time(); x = m_expression_list_x.evaluate(m_expr_x_index); m_expression_variables_y.mv_variable[0].m_value= p_env->get_elapsed_time(); y = m_expression_list_y.evaluate(m_expr_y_index); m_expression_variables_z.mv_variable[0].m_value= p_env->get_elapsed_time(); z = m_expression_list_z.evaluate(m_expr_z_index); Vec3d vec; vec.set(x,y,z); CPositionAttitudeTransform *p_PAT = p_env->get_PAT(m_index_PAT); if(p_PAT!= NULL) p_PAT->mp_PAT->setPosition(vec); return true; }
void drawBlock( Uint16 shape, Uint8 orientation, Uint8 ix, Uint8 iy, Uint8 iz ){ Mat3d rotmat = rotations[ orientation & 0b00011111 ]; rotmat.a.mul( scaling.x ); rotmat.b.mul( scaling.y ); rotmat.c.mul( scaling.z ); if (orientation & 0b10000000) rotmat.a.mul( -1.0d ); if (orientation & 0b01000000) rotmat.b.mul( -1.0d ); if (orientation & 0b00100000) rotmat.c.mul( -1.0d ); Vec3d pos; pos.set( ix*scaling.x+pos0.x, iy*scaling.y+pos0.y, iz*scaling.z+pos0.z ); float glMat[16]; glPushMatrix ( ); /* printf( " pos (%3.3f,%3.3f,%3.3f) \n", pos.x, pos.y, pos.z ); printf( " a (%3.3f,%3.3f,%3.3f) \n", rotmat.a.x, rotmat.a.x, rotmat.a.x ); printf( " b (%3.3f,%3.3f,%3.3f) \n", rotmat.b.x, rotmat.b.x, rotmat.b.x ); printf( " c (%3.3f,%3.3f,%3.3f) \n", rotmat.c.x, rotmat.c.x, rotmat.c.x ); */ Draw3D::toGLMat( pos, rotmat, glMat ); glMultMatrixf( glMat ); glCallList ( shape ); glPopMatrix ( ); }
void getPlaneWaveDescriptor( double * center_, int np, double * points, int nk, double * ks, double * coefs ){ Vec3d center; center.set( center_[0], center_[1], center_[2] ); return getPlaneWaveDescriptor( center, np, (Vec3d*)points, nk, (Vec3d*)ks, coefs ); }
void Yiq::getYIQ(Vec3d& hhh) const { hhh.set(yiq[Y],yiq[I],yiq[Q]); }
void Gun::set_direction( Vec3d dir ){ Vec3d up; up.set( 0.0d, 0.0d, 1.0d ); lrot.a.set( dir ); lrot.c.set_cross( lrot.a, up ); lrot.b.set_cross( lrot.c, lrot.a ); }
void Atmosphere::computeColor (double JD, Vec3d sunPos, Vec3d moonPos, float moonPhase, StelCore* core, float eclipseFac, float latitude, float altitude, float temperature, float relativeHumidity) { // We lazily initialize vertex buffer at the first draw, // so we can only call this after that. // We also need a renderer reference (again lazily from draw()) to // construct index buffers as they might change at every call to computeColor(). if(NULL == renderer) {return;} const StelProjectorP prj = core->getProjection(StelCore::FrameAltAz, StelCore::RefractionOff); if (viewport != prj->getViewport()) { // The viewport changed: update the number of points in the grid updateGrid(prj); } eclipseFactor = eclipseFac; if(eclipseFac < 0.0001f) eclipseFactor = 0.0001f; // No need to calculate if not visible if (!fader.getInterstate()) { averageLuminance = 0.001f + lightPollutionLuminance; return; } // Calculate the atmosphere RGB for each point of the grid if (myisnan(sunPos.length())) sunPos.set(0.,0.,-1.*AU); if (myisnan(moonPos.length())) moonPos.set(0.,0.,-1.*AU); sunPos.normalize(); moonPos.normalize(); float sun_pos[3]; sun_pos[0] = sunPos[0]; sun_pos[1] = sunPos[1]; sun_pos[2] = sunPos[2]; float moon_pos[3]; moon_pos[0] = moonPos[0]; moon_pos[1] = moonPos[1]; moon_pos[2] = moonPos[2]; sky.setParamsv(sun_pos, 5.f); skyb.setLocation(latitude * M_PI/180., altitude, temperature, relativeHumidity); skyb.setSunMoon(moon_pos[2], sun_pos[2]); // Calculate the date from the julian day. int year, month, day; StelUtils::getDateFromJulianDay(JD, &year, &month, &day); skyb.setDate(year, month, moonPhase); // Variables used to compute the average sky luminance double sum_lum = 0.; Vec3d point(1., 0., 0.); skylightStruct2 b2; float lumi; vertexGrid->unlock(); // Compute the sky color for every point above the ground for (int i=0; i<(1+skyResolutionX)*(1+skyResolutionY); ++i) { const Vec2f position = vertexGrid->getVertex(i).position; prj->unProject(position[0], position[1], point); Q_ASSERT(fabs(point.lengthSquared()-1.0) < 1e-10); if (point[2]<=0) { point[2] = -point[2]; // The sky below the ground is the symmetric of the one above : // it looks nice and gives proper values for brightness estimation } // Use the Skybright.cpp 's models for brightness which gives better results. lumi = skyb.getLuminance(moon_pos[0]*point[0]+moon_pos[1]*point[1]+moon_pos[2]*point[2], sun_pos[0]*point[0]+sun_pos[1]*point[1]+sun_pos[2]*point[2], point[2]); lumi *= eclipseFactor; // Add star background luminance lumi += 0.0001; // Multiply by the input scale of the ToneConverter (is not done automatically by the xyYtoRGB method called later) //lumi*=eye->getInputScale(); // Add the light pollution luminance AFTER the scaling to avoid scaling it because it is the cause // of the scaling itself lumi += lightPollutionLuminance; // Store for later statistics sum_lum+=lumi; Q_ASSERT_X(NULL != vertexGrid, Q_FUNC_INFO, "Vertex buffer not initialized when setting colors"); // Now need to compute the xy part of the color component // This is done in a GLSL shader if possible if(NULL != shader) { // Store the back projected position + luminance in the input color to the shader const Vec4f color = Vec4f(point[0], point[1], point[2], lumi); vertexGrid->setVertex(i, Vertex(position, color)); continue; } // Shaderless fallback if (lumi > 0.01) { // Use the Skylight model for the color b2.pos[0] = point[0]; b2.pos[1] = point[1]; b2.pos[2] = point[2]; sky.getxyYValuev(b2); } else { // Too dark to see atmosphere color, don't bother computing it b2.color[0] = 0.25; b2.color[1] = 0.25; } const Vec4f color = Vec4f(b2.color[0], b2.color[1], lumi, 1.0f); vertexGrid->setVertex(i, Vertex(position, color)); } vertexGrid->lock(); // Update average luminance averageLuminance = sum_lum/((1+skyResolutionX)*(1+skyResolutionY)); }