/// sets the servo angles for MAVLink, note angles are in degrees void AP_Mount::set_control_angles(float roll, float tilt, float pan) { _control_angles = Vector3f(roll, tilt, pan); }
void Sphere::tesselate( int nTheta, int nPhi, std::vector< Vector3f >& positions, std::vector< Vector3f >& normals ) { positions.clear(); normals.clear(); positions.reserve( 6 * nTheta * nPhi ); normals.reserve( 6 * nTheta * nPhi ); float dt = libcgt::core::math::TWO_PI / nTheta; float dp = libcgt::core::math::PI / nPhi; Vector3f c( center ); for( int t = 0; t < nTheta; ++t ) { float t0 = t * dt; float t1 = t0 + dt; for( int p = 0; p < nPhi; ++p ) { float p0 = p * dp; float p1 = p0 + dp; float x00 = cosf( t0 ) * sinf( p0 ); float y00 = sinf( t0 ) * sinf( p0 ); float z00 = cosf( p0 ); float x10 = cosf( t1 ) * sinf( p0 ); float y10 = sinf( t1 ) * sinf( p0 ); float z10 = cosf( p0 ); float x01 = cosf( t0 ) * sinf( p1 ); float y01 = sinf( t0 ) * sinf( p1 ); float z01 = cosf( p1 ); float x11 = cosf( t1 ) * sinf( p1 ); float y11 = sinf( t1 ) * sinf( p1 ); float z11 = cosf( p1 ); Vector3f v00 = c + Vector3f( radius * x00, radius * y00, radius * z00 ); Vector3f n00( x00, y00, z00 ); Vector3f v10 = c + Vector3f( radius * x10, radius * y10, radius * z10 ); Vector3f n10( x10, y10, z10 ); Vector3f v01 = c + Vector3f( radius * x01, radius * y01, radius * z01 ); Vector3f n01( x01, y01, z01 ); Vector3f v11 = c + Vector3f( radius * x11, radius * y11, radius * z11 ); Vector3f n11( x11, y11, z11 ); positions.push_back( v00 ); normals.push_back( n00 ); positions.push_back( v10 ); normals.push_back( n10 ); positions.push_back( v01 ); normals.push_back( n01 ); positions.push_back( v01 ); normals.push_back( n01 ); positions.push_back( v10 ); normals.push_back( n10 ); positions.push_back( v11 ); normals.push_back( n11 ); } } }
void pbrtRotate(Float angle, Float dx, Float dy, Float dz) { VERIFY_INITIALIZED("Rotate"); FOR_ACTIVE_TRANSFORMS(curTransform[i] = curTransform[i] * Rotate(angle, Vector3f(dx, dy, dz));) }
Mesh MeshLoader::getPortalBox(const Entity &wall) { // Generate vertex array object GLuint vao = 0; glGenVertexArrays(1, &vao); glBindVertexArray(vao); /* == Static part: vertices, normals and tangents == */ constexpr unsigned int coordsSize = sizeof(float) * 3, texcSize = sizeof(float) * 2, normalsSize = sizeof(int8_t) * 3, tangentsSize = sizeof(int8_t) * 3, vtxSize = coordsSize + texcSize + normalsSize + tangentsSize, vboSize = vtxSize * 3 /*verts*/ * 2 /*tris*/ * 6; /*faces*/ TightDataPacker data(vboSize); static constexpr float vertices[8][3] = { {-0.5f, -0.5f, -0.5f}, {-0.5f, -0.5f, 0.5f}, {-0.5f, 0.5f, -0.5f}, {-0.5f, 0.5f, 0.5f}, {0.5f, -0.5f, -0.5f}, {0.5f, -0.5f, 0.5f}, {0.5f, 0.5f, -0.5f}, {0.5f, 0.5f, 0.5f}}; static constexpr uint8_t vi[36] = { 3, 1, 5, 3, 5, 7, // Front 7, 5, 4, 7, 4, 6, // Left 6, 4, 0, 6, 0, 2, // Back 2, 0, 1, 2, 1, 3, // Right 2, 3, 7, 2, 7, 6, // Top 1, 0, 4, 1, 4, 5 // Bottom }; const Vector3f &scale = wall.getScale(); const float texCoords[8][2] = { {0, 0}, {scale.x, 0}, {scale.z, 0}, {0, scale.y}, {0, scale.z}, {scale.x, scale.y}, {scale.x, scale.z}, {scale.z, scale.y}}; static constexpr uint8_t ti[36] = {0, 3, 5, 0, 5, 1, 0, 3, 7, 0, 7, 2, 0, 3, 5, 0, 5, 1, 0, 3, 7, 0, 7, 2, 0, 4, 6, 0, 6, 1, 0, 4, 6, 0, 6, 1}; static constexpr int8_t normals[6][3] = {{0, 0, 1}, {1, 0, 0}, {0, 0, -1}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}}; static constexpr uint8_t ni[36] = {0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5}; static const int8_t tangents[6][3] = {{0, 1, 0}, {0, 0, 1}, {0, -1, 0}, {0, 0, -1}, {1, 0, 0}, {-1, 0, 0}}; static constexpr uint8_t tai[36] = {0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5}; constexpr unsigned int texCoordsOffset = coordsSize, normalsOffset = texCoordsOffset + texcSize, tangentsOffset = normalsOffset + normalsSize; for (int i = 0; i < 36; ++i) { const float *v = vertices[vi[i]]; data << v[0] << v[1] << v[2]; const float *t = texCoords[ti[i]]; data << t[0] << t[1]; const int8_t *n = normals[ni[i]]; data << n[0] << n[1] << n[2]; const int8_t *ta = tangents[tai[i]]; data << ta[0] << ta[1] << ta[2]; } Mesh mesh; mesh.vertices.resize(8); for (unsigned int i = 0; i < 8; ++i) { const float *v = vertices[i]; mesh.vertices[i] = Vector3f(v[0], v[1], v[2]); } assert(data.getSize() == vboSize); createVBO(GL_ARRAY_BUFFER, vboSize, data.getDataPtr(), vtxSize, {{0, 3, GL_FLOAT, nullptr}, // Vertices {1, 2, GL_FLOAT, reinterpret_cast<GLvoid *>(texCoordsOffset)}, // Tex coords {2, 3, GL_BYTE, reinterpret_cast<GLvoid *>(normalsOffset)}, // Normals {3, 3, GL_BYTE, reinterpret_cast<GLvoid *>(tangentsOffset)}}); // Tangents // unbind Vertex Array Object (VAO) glBindVertexArray(0); // Store relevant data in the new mesh mesh.handle = static_cast<int>(vao); mesh.numFaces = 12; return mesh; }
Terrain::Terrain(float longueur, float largeur, float amplitude): longueur(longueur), largeur(largeur) { box = Box(Vector3f(0,0,0),Vector3f(largeur,longueur,amplitude)); heatMapGradient.createDefaultHeatMapGradient(); }
Vector3f PinguAction::get_center_pos() const { return pingu->get_pos() + Vector3f(0, -16); }
Quatf Player::GetOrientation(bool baseOnly) { Quatf baseQ = Quatf(Vector3f(0,1,0), BodyYaw.Get()); return baseOnly ? baseQ : baseQ * HeadPose.Rotation; }
Vector3f Rectangle::WorldCentroid() const { return TransformPoint(local_to_world, Vector3f(0.f, 0.f, 0.f)); }
Vector3f normalize( Vector3f a) { float len = length(a); return Vector3f( a.x/len, a.y/len, a.z/len); }
/** operator * Transforms a point by this matrix. */ Vector3f Matrix4x4::operator * (Vector3f& p) { return Vector3f(_mat[0] * p[0] + _mat[4] * p[1] + _mat[8] * p[2] + _mat[12], _mat[1] * p[0] + _mat[5] * p[1] + _mat[9] * p[2] + _mat[13], _mat[2] * p[0] + _mat[6] * p[1] + _mat[10] * p[2] + _mat[14]); }
BBOX Rectangle::LocalBBOX() const { return BBOX(Vector3f(-x_size / 2.f, EPS, -z_size / 2.f), Vector3f(x_size / 2.f, -EPS, z_size / 2.f)); }
Vector2i Obj::load_heightmap(string file_name, float scale) { int width = 0, height = 0; string line; ifstream myfile (file_name.c_str()); if (myfile.is_open()) { while (! myfile.eof() ) { getline (myfile,line); int this_width = 0; for (string::iterator it=line.begin() ; it < line.end(); it++ ) if (*it == ',') this_width++; if (width == 0) { width = this_width; } else if (this_width == 0) { break; } else if (this_width != width) { cerr << "all lines in the heightmap must be the same length" << endl; } height++; } myfile.close(); } else { cerr << "could not open file " << file_name << endl; } int size = width*height; vertices = new Vertex[size]; v_poss = new Vector3f[size]; v_norms = new Vector3f[size]; v_texts = new Vector2f[size]; face_count = (width-1)*(height-1); faces = new Face[(width-1)*(height-1)]; //myfile.open(file_name.c_str()); ifstream myfile2 (file_name.c_str()); for (int i = 0 ; i < height ; i++) { getline (myfile2,line); string tmp = ""; int j = 0; for (string::iterator it=line.begin() ; it < line.end(); it++ ) { if (*it == ',') { int index = i*width+j; v_poss[index].init((i/*-((float)width)/2.0f*/)*scale,atof(tmp.c_str())*scale,(j/*-((float)height)/2.0f*/)*scale); v_texts[index].init(i,j); v_norms[index].init(0,1,0); vertices[index].pos = &v_poss[index]; vertices[index].text = &v_texts[index]; vertices[index].normal = &v_norms[index]; tmp = ""; j++; } else { tmp += *it; } } } myfile2.close(); for (int i = 0; i < height-1 ; i++) { for (int j = 0; j < width-1 ; j++) { if (i > 0 && j > 0) { int index = i*width+j; //cout << "setting normal at:" << v_poss[index] << endl; Vector3f norm_i = v_poss[(i-1)*width+j].dir_between(v_poss[(i+1)*width+j]); //cout << "i dir " << norm_i << endl; norm_i.rotate(Vector3f(0,0,90)); //cout << "i dir " << norm_i << endl; norm_i.point_up(); //cout << "i dir " << norm_i << endl; Vector3f norm_j = v_poss[i*width+(j-1)].dir_between(v_poss[i*width+(j+1)]); //cout << "j dir " << norm_j << endl; norm_j.rotate(Vector3f(90,0,0)); //cout << "j dir " << norm_j << endl; norm_j.point_up(); //cout << "j dir " << norm_j << endl; v_norms[index] = (norm_i + norm_j); //cout << "normal " << v_norms[index] << endl; v_norms[index].normalise(); //cout << "normal " << v_norms[index] << endl; vertices[index].normal = &v_norms[index]; //cout << "set normal at " << v_poss[index] << " to " << v_norms[index] << endl; } int f_index = i*(width-1)+j; faces[f_index].vertex_count = 4; faces[f_index].vertices = new Vertex[4]; faces[f_index].vertices[0] = vertices[i*width+j]; faces[f_index].vertices[1] = vertices[i*width+(j+1)]; faces[f_index].vertices[2] = vertices[(i+1)*width+(j+1)]; faces[f_index].vertices[3] = vertices[(i+1)*width+j]; } } Vector2i map_size(width,height); return map_size; }
bool Paraboloid::Intersect(const Ray &r, Float *tHit, SurfaceInteraction *isect) const { Float phi; Point3f pHit; // Transform _Ray_ to object space Vector3f oErr, dErr; Ray ray = (*WorldToObject)(r, &oErr, &dErr); // Compute quadratic paraboloid coefficients // Initialize _EFloat_ ray coordinate values EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z); EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z); EFloat k = EFloat(zMax) / (EFloat(radius) * EFloat(radius)); EFloat a = k * (dx * dx + dy * dy); EFloat b = 2.f * k * (dx * ox + dy * oy) - dz; EFloat c = k * (ox * ox + oy * oy) - oz; // Solve quadratic equation for _t_ values EFloat t0, t1; if (!Quadratic(a, b, c, &t0, &t1)) return false; // Check quadric shape _t0_ and _t1_ for nearest intersection if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false; EFloat tShapeHit = t0; if (t0.LowerBound() <= 0) { tShapeHit = t1; if (tShapeHit.UpperBound() > ray.tMax) return false; } // Compute paraboloid inverse mapping pHit = ray((Float)tShapeHit); phi = std::atan2(pHit.y, pHit.x); if (phi < 0.) phi += 2 * Pi; // Test paraboloid intersection against clipping parameters if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) { if (tShapeHit == t1) return false; tShapeHit = t1; if (t1.UpperBound() > ray.tMax) return false; // Compute paraboloid inverse mapping pHit = ray((Float)tShapeHit); phi = std::atan2(pHit.y, pHit.x); if (phi < 0.) phi += 2 * Pi; if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false; } // Find parametric representation of paraboloid hit Float u = phi / phiMax; Float v = (pHit.z - zMin) / (zMax - zMin); // Compute paraboloid $\dpdu$ and $\dpdv$ Vector3f dpdu(-phiMax * pHit.y, phiMax * pHit.x, 0.); Vector3f dpdv = (zMax - zMin) * Vector3f(pHit.x / (2 * pHit.z), pHit.y / (2 * pHit.z), 1.); // Compute paraboloid $\dndu$ and $\dndv$ Vector3f d2Pduu = -phiMax * phiMax * Vector3f(pHit.x, pHit.y, 0); Vector3f d2Pduv = (zMax - zMin) * phiMax * Vector3f(-pHit.y / (2 * pHit.z), pHit.x / (2 * pHit.z), 0); Vector3f d2Pdvv = -(zMax - zMin) * (zMax - zMin) * Vector3f(pHit.x / (4 * pHit.z * pHit.z), pHit.y / (4 * pHit.z * pHit.z), 0.); // Compute coefficients for fundamental forms Float E = Dot(dpdu, dpdu); Float F = Dot(dpdu, dpdv); Float G = Dot(dpdv, dpdv); Vector3f N = Normalize(Cross(dpdu, dpdv)); Float e = Dot(N, d2Pduu); Float f = Dot(N, d2Pduv); Float g = Dot(N, d2Pdvv); // Compute $\dndu$ and $\dndv$ from fundamental form coefficients Float invEGF2 = 1 / (E * G - F * F); Normal3f dndu = Normal3f((f * F - e * G) * invEGF2 * dpdu + (e * F - f * E) * invEGF2 * dpdv); Normal3f dndv = Normal3f((g * F - f * G) * invEGF2 * dpdu + (f * F - g * E) * invEGF2 * dpdv); // Compute error bounds for paraboloid intersection // Compute error bounds for intersection computed with ray equation EFloat px = ox + tShapeHit * dx; EFloat py = oy + tShapeHit * dy; EFloat pz = oz + tShapeHit * dz; Vector3f pError = Vector3f(px.GetAbsoluteError(), py.GetAbsoluteError(), pz.GetAbsoluteError()); // Initialize _SurfaceInteraction_ from parametric information *isect = (*ObjectToWorld)(SurfaceInteraction(pHit, pError, Point2f(u, v), -ray.d, dpdu, dpdv, dndu, dndv, ray.time, this)); *tHit = (Float)tShapeHit; return true; }
// Recursively traverses the assimp node hierarchy, accumulating // modeling transformations, and creating and transforming any meshes // found. Meshes comming from assimp can have associated surface // properties, so each mesh *copies* the current BRDF as a starting // point and modifies it from the assimp data structure. void recurseModelNodes(Scene* scene, const aiScene* aiscene, const aiNode* node, const aiMatrix4x4& parentTr, const int level) { // Print line with indentation to show structure of the model node hierarchy. for (int i=0; i<level; i++) printf("| "); printf("%s ", node->mName.data); // Accumulating transformations while traversing down the hierarchy. aiMatrix4x4 childTr = parentTr*node->mTransformation; aiMatrix3x3 normalTr = aiMatrix3x3(childTr); // Really should be inverse-transpose for full generality // Loop through this node's meshes for (unsigned int i=0; i<node->mNumMeshes; ++i) { aiMesh* aimesh = aiscene->mMeshes[node->mMeshes[i]]; printf("%d:%d ", aimesh->mNumVertices, aimesh->mNumFaces); // Extract this node's surface material. aiString texPath; aiMaterial* mtl = aiscene->mMaterials[aimesh->mMaterialIndex]; // Assimp can read material colors from the input files, but // it seems to invent colors of its own unpredictably so I // ignore them. Textures and texture coordinates seem to work // well. //aiColor3D diff (0.f,0.f,0.f); //aiColor3D spec (0.f,0.f,0.f); //float s; // if (AI_SUCCESS == mtl->Get(AI_MATKEY_COLOR_DIFFUSE, diff)) // scene->setKd(Vector3f(diff.r, diff.g, diff.b)); // if (AI_SUCCESS == mtl->Get(AI_MATKEY_COLOR_SPECULAR, spec)) // scene->setKs(Vector3f(spec.r, spec.g, spec.b)); // if (AI_SUCCESS == mtl->Get(AI_MATKEY_SHININESS, &s, NULL)) // scene->setAlpha(s); Material *material = new Material(*scene->currentMat); if (AI_SUCCESS == mtl->GetTexture(aiTextureType_DIFFUSE, 0, &texPath)) material->setTexture(texPath.C_Str()); // Arrays to hold all vertex and triangle data. MeshData* meshdata = new MeshData; // Loop through all vertices and record the // vertex/normal/texture/tangent data with the node's model // transformation applied. for (unsigned int t=0; t<aimesh->mNumVertices; ++t) { aiVector3D aipnt = childTr*aimesh->mVertices[t]; aiVector3D ainrm = aimesh->HasNormals() ? normalTr*aimesh->mNormals[t] : aiVector3D(0,0,1); aiVector3D aitex = aimesh->HasTextureCoords(0) ? aimesh->mTextureCoords[0][t] : aiVector3D(0,0,0); aiVector3D aitan = aimesh->HasTangentsAndBitangents() ? normalTr*aimesh->mTangents[t] : aiVector3D(1,0,0); meshdata->vertices.push_back(VertexData(Vector3f(aipnt.x, aipnt.y, aipnt.z), Vector3f(ainrm.x, ainrm.y, ainrm.z), Vector2f(aitex.x, aitex.y), Vector3f(aitan.x, aitan.y, aitan.z))); } // Loop through all faces, recording indices for (unsigned int t=0; t<aimesh->mNumFaces; ++t) { aiFace* aiface = &aimesh->mFaces[t]; meshdata->triangles.push_back(TriData(aiface->mIndices[0], aiface->mIndices[1], aiface->mIndices[2])); } meshdata->mat = material; scene->triangleMesh(meshdata); delete meshdata; } printf("\n"); // Recurse onto this node's children for (unsigned int i=0; i<node->mNumChildren; ++i) recurseModelNodes(scene, aiscene, node->mChildren[i], childTr, level+1); }
Enclosure::Enclosure(engine::IClump* clump, float delay) { _clump = clump; _clump->getFrame()->translate( Vector3f( 0,0,0 ) ); _clump->getFrame()->getLTM(); _delay = delay; _ray = Gameplay::iEngine->createRayIntersection(); _sphere = Gameplay::iEngine->createSphereIntersection(); callback::Locator locator; locator.targetName = _clump->getName(); locator.target = NULL; _clump->forAllAtomics( callback::locateAtomic, &locator ); _collisionAtomic = reinterpret_cast<engine::IAtomic*>( locator.target ); assert( _collisionAtomic ); engine::IGeometry* geometry = _collisionAtomic->getGeometry(); engine::IShader* shader; unsigned int numFaces = geometry->getNumFaces(); Vector3f vertex[3]; Vector3f normal; bool flag; unsigned int i,j; // enumerate normals of wall faces for( i=0; i<numFaces; i++ ) { geometry->getFace( i, vertex[0], vertex[1], vertex[2], &shader ); if( strcmp( shader->getName(), "EnclosureFloor" ) != 0 ) { normal.cross( vertex[1]-vertex[0], vertex[2]-vertex[0] ); normal.normalize(); // check this normal was enumerated flag = false; for( j=0; j<_wallNormals.size(); j++ ) { if( Vector3f::dot( _wallNormals[j], normal ) > 0.999f ) { flag = true; break; } } if( !flag ) _wallNormals.push_back( normal ); } } // enumerate position markers (all atomics under enclosure care) callback::AtomicL atomicL; _clump->forAllAtomics( callback::enumerateAtomics, &atomicL ); for( callback::AtomicI atomicI=atomicL.begin(); atomicI!=atomicL.end(); atomicI++ ) { if( (*atomicI) != _collisionAtomic ) { _markers.push_back( (*atomicI)->getFrame() ); } } /* // check number of path points (limit of 2) unsigned int numPathPoints = 0; for( unsigned int i=0; i<getNumMarkers(); i++ ) { if( getMarkerFlags( i ) & mtPath ) numPathPoints++; } if( numPathPoints > 2 ) { throw Exception( "Too many pathpoints in enclosure %s", clump->getName() ); } */ }
#include "RotateAroundAxisNode.h" #include "../../../Math/Lower Math/Quaternion.h" ADD_NODE_REFLECTION_DATA_CPP(RotateAroundAxisNode, Vector3f(1.0f, 0.0f, 0.0f), Vector3f(0.0f, 0.0f, 1.0f), 0.0f) RotateAroundAxisNode::RotateAroundAxisNode(const DataLine & toRot, const DataLine & axis, const DataLine & angle, std::string name) : DataNode(MakeVector(toRot, axis, angle), name) { } void RotateAroundAxisNode::GetMyFunctionDeclarations(std::vector<std::string> & outDecls) const { std::string rotFunc = "vec3 " + GetName() + "_rotFunc(vec3 toRot, vec3 rotAxis, float rotAmount) \n\ { \n\ float halfAng = rotAmount * 0.5f; \n\ vec3 quatXYZ = sin(halfAng) * rotAxis; \n\ return toRot + (2.0f * cross(quatXYZ, cross(quatXYZ, toRot) + (toRot * cos(halfAng)))); \n\ }\n"; outDecls.insert(outDecls.end(), rotFunc); } void RotateAroundAxisNode::WriteMyOutputs(std::string & outCode) const { outCode += "\tvec3 " + GetOutputName(0) + " = " + GetName() + "_rotFunc(" + GetToRotateInput().GetValue() + ", " + GetRotateAxisInput().GetValue() + ", " + GetRotateAmountInput().GetValue() + ");\n"; }
void PinguAction::move_with_forces () { // Apply gravity pingu->set_velocity(pingu->get_velocity() + Vector3f(0.0f, 1.0f)); #if 0 // New Code Vector3f pos = pingu->get_pos(); Vector3f target_pos = pos + pingu->get_velocity(); Vector3f dir = target_pos - pingu->get_pos(); Vector3f velocity = pingu->get_velocity(); float length = dir.length(); dir.normalize(); for(float i = 0; i < length; ++i) { pingu->set_pos(pos + (dir * i)); // If there is something below the Pingu if (rel_getpixel(0, -1) != Groundtype::GP_NOTHING) { // FIXME: this shouldn't be really here, but its a // FIXME: quick&dirty way to kill falling pingus if (velocity.y > Actions::Faller::deadly_velocity+1) { // log_debug("Velocity: " << velocity); pingu->set_action(Actions::Splashed); return; } else { // Make it so that the Pingu won't go down any further. pingu->set_velocity(Vector3f(0, 0)); return; } } else if (head_collision_on_walk(0, 1)) { return; } else if (collision_on_walk(1, 0)) { // Make the Pingu bounce off the wall velocity.x = -velocity.x / 3.0f; pingu->set_velocity(velocity); pingu->direction.change(); return; } } #else // Old Code // FIXME: What does this variable do? Vector3f resultant_force = pingu->get_velocity(); // FIXME: and what is this all about?! Can't we just use floats? // Strictly speaking x_numerator should be initialised with // (resultant_force.y / 2) and y_numerator with (resultant_force.x / 2). // This would make the algorithm essentially match the Mid-Point Line // Algorithm. However, zero should do and is more comprehendable. int x_numerator = 0; int y_numerator = 0; int denominator = 0; int x_inc = 0; int y_inc = 0; if (Math::abs(resultant_force.x) > Math::abs(resultant_force.y)) { // Initialise so that we move in whole pixels in x direction and // 'fractions' of a pixel in y direction. denominator = static_cast<int>(Math::abs(resultant_force.x)); x_inc = denominator; y_inc = static_cast<int>(Math::abs(resultant_force.y)); } else { // Initialise so that we move in whole pixels in y direction and // 'fractions' of a pixel in x direction. denominator = static_cast<int>(Math::abs(resultant_force.y)); x_inc = static_cast<int>(Math::abs(resultant_force.x)); y_inc = denominator; } Vector3f force_counter = resultant_force; // Keep moving the Pingu until there is only a fraction left while ( force_counter.x <= -1 || force_counter.x >= 1 || force_counter.y <= -1 || force_counter.y >= 1) { x_numerator += x_inc; // Is it now not a fraction? if (x_numerator >= denominator) { // Revert back to being a fraction x_numerator -= denominator; // If there is something to the left of the Pingu if (collision_on_walk(1, 0)) { // Make the Pingu reflect off the wall force_counter.x = -(force_counter.x); resultant_force.x = -(resultant_force.x/3); pingu->set_velocity(resultant_force); pingu->direction.change(); } else { // Move the Pingu left pingu->set_x(pingu->get_x() + static_cast<float>(pingu->direction)); force_counter.x -= static_cast<float>(pingu->direction); } } y_numerator += y_inc; // Is it now not a fraction? if (y_numerator >= denominator) { // Revert back to being a fraction y_numerator -= denominator; // Move the Pingu depending on what the direction of the force is if (force_counter.y >= 1) { // If there is something below the Pingu if (rel_getpixel(0, -1) != Groundtype::GP_NOTHING) { // FIXME: this shouldn't be really here, but its a // FIXME: quick&dirty way to kill falling pingus if (resultant_force.y >= deadly_velocity) { pingu->set_action(ActionName::SPLASHED); return; } // Make it so that the Pingu won't go down any further. pingu->set_velocity(Vector3f(0, 0)); return; } else { // Move the Pingu down pingu->set_y(pingu->get_y() + 1); force_counter.y--; } } else if (force_counter.y <= -1) { // If there is something in the way above the Pingu if (head_collision_on_walk(0, 1)) { // Make it so that the Pingu won't go up any further. force_counter.y = 0; resultant_force.y = 0; pingu->set_velocity(resultant_force); } else { // Move the Pingu up pingu->set_y(pingu->get_y() - 1); force_counter.y++; } } } } #endif }
Vector3f get_pos() const { return Vector3f(); }
Vector3f Player::GetPosition() { return BodyPos + Quatf(Vector3f(0,1,0), BodyYaw.Get()).Rotate(HeadPose.Translation); }
//----------------------------------------------------------------------------// void BasicImage::render(GeometryBuffer& buffer, const Rectf& dest_area, const Rectf* clip_area, const ColourRect& colours) const { const QuadSplitMode quad_split_mode(TopLeftToBottomRight); Rectf dest(dest_area); // apply rendering offset to the destination Rect dest.offset(d_scaledOffset); // get the rect area that we will actually draw to (i.e. perform clipping) Rectf final_rect(clip_area ? dest.getIntersection(*clip_area) : dest ); // check if rect was totally clipped if ((final_rect.getWidth() == 0) || (final_rect.getHeight() == 0)) return; // Obtain correct scale values from the texture const Vector2f& scale = d_texture->getTexelScaling(); const Vector2f tex_per_pix(d_area.getWidth() / dest.getWidth(), d_area.getHeight() / dest.getHeight()); // calculate final, clipped, texture co-ordinates const Rectf tex_rect((d_area.d_min + ((final_rect.d_min - dest.d_min) * tex_per_pix)) * scale, (d_area.d_max + ((final_rect.d_max - dest.d_max) * tex_per_pix)) * scale); // URGENT FIXME: Shouldn't this be in the hands of the user? final_rect.d_min.d_x = CoordConverter::alignToPixels(final_rect.d_min.d_x); final_rect.d_min.d_y = CoordConverter::alignToPixels(final_rect.d_min.d_y); final_rect.d_max.d_x = CoordConverter::alignToPixels(final_rect.d_max.d_x); final_rect.d_max.d_y = CoordConverter::alignToPixels(final_rect.d_max.d_y); Vertex vbuffer[6]; // vertex 0 vbuffer[0].position = Vector3f(final_rect.left(), final_rect.top(), 0.0f); vbuffer[0].colour_val = colours.d_top_left; vbuffer[0].tex_coords = Vector2f(tex_rect.left(), tex_rect.top()); // vertex 1 vbuffer[1].position = Vector3f(final_rect.left(), final_rect.bottom(), 0.0f); vbuffer[1].colour_val = colours.d_bottom_left; vbuffer[1].tex_coords = Vector2f(tex_rect.left(), tex_rect.bottom()); // vertex 2 vbuffer[2].position.d_x = final_rect.right(); vbuffer[2].position.d_z = 0.0f; vbuffer[2].colour_val = colours.d_bottom_right; vbuffer[2].tex_coords.d_x = tex_rect.right(); // top-left to bottom-right diagonal if (quad_split_mode == TopLeftToBottomRight) { vbuffer[2].position.d_y = final_rect.bottom(); vbuffer[2].tex_coords.d_y = tex_rect.bottom(); } // bottom-left to top-right diagonal else { vbuffer[2].position.d_y = final_rect.top(); vbuffer[2].tex_coords.d_y = tex_rect.top(); } // vertex 3 vbuffer[3].position = Vector3f(final_rect.right(), final_rect.top(), 0.0f); vbuffer[3].colour_val = colours.d_top_right; vbuffer[3].tex_coords = Vector2f(tex_rect.right(), tex_rect.top()); // vertex 4 vbuffer[4].position.d_x = final_rect.left(); vbuffer[4].position.d_z = 0.0f; vbuffer[4].colour_val = colours.d_top_left; vbuffer[4].tex_coords.d_x = tex_rect.left(); // top-left to bottom-right diagonal if (quad_split_mode == TopLeftToBottomRight) { vbuffer[4].position.d_y = final_rect.top(); vbuffer[4].tex_coords.d_y = tex_rect.top(); } // bottom-left to top-right diagonal else { vbuffer[4].position.d_y = final_rect.bottom(); vbuffer[4].tex_coords.d_y = tex_rect.bottom(); } // vertex 5 vbuffer[5].position = Vector3f(final_rect.right(), final_rect.bottom(), 0.0f); vbuffer[5].colour_val= colours.d_bottom_right; vbuffer[5].tex_coords = Vector2f(tex_rect.right(), tex_rect.bottom()); buffer.setActiveTexture(d_texture); buffer.appendGeometry(vbuffer, 6); }
void Player::HandleMovement(double dt, Array<Ptr<CollisionModel> >* collisionModels, Array<Ptr<CollisionModel> >* groundCollisionModels, bool shiftDown) { // Handle keyboard movement. // This translates BasePos based on the orientation and keys pressed. // Note that Pitch and Roll do not affect movement (they only affect view). Vector3f controllerMove; if(MoveForward || MoveBack || MoveLeft || MoveRight) { if (MoveForward) { controllerMove += ForwardVector; } else if (MoveBack) { controllerMove -= ForwardVector; } if (MoveRight) { controllerMove += RightVector; } else if (MoveLeft) { controllerMove -= RightVector; } } else if (GamepadMove.LengthSq() > 0) { controllerMove = GamepadMove; } controllerMove = GetOrientation(bMotionRelativeToBody).Rotate(controllerMove); controllerMove.y = 0; // Project to the horizontal plane if (controllerMove.LengthSq() > 0) { // Normalize vector so we don't move faster diagonally. controllerMove.Normalize(); controllerMove *= OVR::Alg::Min<float>(MoveSpeed * (float)dt * (shiftDown ? 3.0f : 1.0f), 1.0f); } // Compute total move direction vector and move length Vector3f orientationVector = controllerMove; float moveLength = orientationVector.Length(); if (moveLength > 0) orientationVector.Normalize(); float checkLengthForward = moveLength; Planef collisionPlaneForward; bool gotCollision = false; for(unsigned int i = 0; i < collisionModels->GetSize(); ++i) { // Checks for collisions at model base level, which should prevent us from // slipping under walls if (collisionModels->At(i)->TestRay(BodyPos, orientationVector, checkLengthForward, &collisionPlaneForward)) { gotCollision = true; break; } } if (gotCollision) { // Project orientationVector onto the plane Vector3f slideVector = orientationVector - collisionPlaneForward.N * (orientationVector.Dot(collisionPlaneForward.N)); // Make sure we aren't in a corner for(unsigned int j = 0; j < collisionModels->GetSize(); ++j) { if (collisionModels->At(j)->TestPoint(BodyPos - Vector3f(0.0f, RailHeight, 0.0f) + (slideVector * (moveLength))) ) { moveLength = 0; break; } } if (moveLength != 0) { orientationVector = slideVector; } } // Checks for collisions at foot level, which allows us to follow terrain orientationVector *= moveLength; BodyPos += orientationVector; Planef collisionPlaneDown; float finalDistanceDown = GetScaledEyeHeight() + 10.0f; // Only apply down if there is collision model (otherwise we get jitter). if (groundCollisionModels->GetSize()) { for(unsigned int i = 0; i < groundCollisionModels->GetSize(); ++i) { float checkLengthDown = GetScaledEyeHeight() + 10; if (groundCollisionModels->At(i)->TestRay(BodyPos, Vector3f(0.0f, -1.0f, 0.0f), checkLengthDown, &collisionPlaneDown)) { finalDistanceDown = Alg::Min(finalDistanceDown, checkLengthDown); } } // Maintain the minimum camera height if (GetScaledEyeHeight() - finalDistanceDown < 1.0f) { BodyPos.y += GetScaledEyeHeight() - finalDistanceDown; } } }
bool DeferredPhongLightingPass::Process(const SceneData& sceneData, unsigned int firstWorkTexture, unsigned secondWorkTexture) const { NazaraAssert(sceneData.viewer, "Invalid viewer"); NazaraUnused(secondWorkTexture); m_workRTT->SetColorTarget(firstWorkTexture); Renderer::SetTarget(m_workRTT); Renderer::SetViewport(Recti(0, 0, m_dimensions.x, m_dimensions.y)); Renderer::SetTexture(0, m_GBuffer[0]); Renderer::SetTextureSampler(0, m_pointSampler); Renderer::SetTexture(1, m_GBuffer[1]); Renderer::SetTextureSampler(1, m_pointSampler); Renderer::SetTexture(2, m_GBuffer[2]); Renderer::SetTextureSampler(2, m_pointSampler); Renderer::SetTexture(3, m_depthStencilTexture); Renderer::SetTextureSampler(3, m_pointSampler); Renderer::SetClearColor(Color::Black); Renderer::Clear(RendererBuffer_Color); RenderStates lightStates; lightStates.dstBlend = BlendFunc_One; lightStates.srcBlend = BlendFunc_One; lightStates.blending = true; lightStates.depthBuffer = false; lightStates.depthWrite = false; // Directional lights if (!m_renderQueue->directionalLights.empty()) { Renderer::SetRenderStates(lightStates); Renderer::SetShader(m_directionalLightShader); m_directionalLightShader->SendColor(m_directionalLightShaderSceneAmbientLocation, sceneData.ambientColor); m_directionalLightShader->SendVector(m_directionalLightShaderEyePositionLocation, sceneData.viewer->GetEyePosition()); for (auto& light : m_renderQueue->directionalLights) { m_directionalLightShader->SendColor(m_directionalLightUniforms.locations.color, light.color); m_directionalLightShader->SendVector(m_directionalLightUniforms.locations.factors, Vector2f(light.ambientFactor, light.diffuseFactor)); m_directionalLightShader->SendVector(m_directionalLightUniforms.locations.parameters1, Vector4f(light.direction)); Renderer::DrawFullscreenQuad(); } } // Point lights/Spot lights if (!m_renderQueue->pointLights.empty() || !m_renderQueue->spotLights.empty()) { // http://www.altdevblogaday.com/2011/08/08/stencil-buffer-optimisation-for-deferred-lights/ lightStates.cullingSide = FaceSide_Front; lightStates.stencilTest = true; lightStates.stencilDepthFail.back = StencilOperation_Invert; lightStates.stencilDepthFail.front = StencilOperation_Invert; lightStates.stencilFail.back = StencilOperation_Keep; lightStates.stencilFail.front = StencilOperation_Keep; lightStates.stencilPass.back = StencilOperation_Keep; lightStates.stencilPass.front = StencilOperation_Keep; lightStates.stencilReference.back = 0; lightStates.stencilReference.front = 0; lightStates.stencilWriteMask.back = 0xFF; lightStates.stencilWriteMask.front = 0xFF; Renderer::SetRenderStates(lightStates); Renderer::SetShader(m_pointSpotLightShader); m_pointSpotLightShader->SendColor(m_pointSpotLightShaderSceneAmbientLocation, sceneData.ambientColor); m_pointSpotLightShader->SendVector(m_pointSpotLightShaderEyePositionLocation, sceneData.viewer->GetEyePosition()); Matrix4f lightMatrix; lightMatrix.MakeIdentity(); if (!m_renderQueue->pointLights.empty()) { const IndexBuffer* indexBuffer = m_sphereMesh->GetIndexBuffer(); Renderer::SetIndexBuffer(indexBuffer); Renderer::SetVertexBuffer(m_sphereMesh->GetVertexBuffer()); m_pointSpotLightShader->SendInteger(m_pointSpotLightUniforms.locations.type, LightType_Point); for (const auto& light : m_renderQueue->pointLights) { m_pointSpotLightShader->SendColor(m_pointSpotLightUniforms.locations.color, light.color); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.factors, Vector2f(light.ambientFactor, light.diffuseFactor)); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters1, Vector4f(light.position, light.attenuation)); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters2, Vector4f(0.f, 0.f, 0.f, light.invRadius)); lightMatrix.SetScale(Vector3f(light.radius * 1.1f)); // To correct imperfections due to the sphere lightMatrix.SetTranslation(light.position); Renderer::SetMatrix(MatrixType_World, lightMatrix); // Sphere rendering in the stencil buffer Renderer::Enable(RendererParameter_ColorWrite, false); Renderer::Enable(RendererParameter_DepthBuffer, true); Renderer::Enable(RendererParameter_FaceCulling, false); Renderer::SetStencilCompareFunction(RendererComparison_Always); m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, true); Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount()); // Sphere rendering as effect zone Renderer::Enable(RendererParameter_ColorWrite, true); Renderer::Enable(RendererParameter_DepthBuffer, false); Renderer::Enable(RendererParameter_FaceCulling, true); Renderer::SetStencilCompareFunction(RendererComparison_NotEqual, FaceSide_Back); Renderer::SetStencilPassOperation(StencilOperation_Zero, FaceSide_Back); m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, false); Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount()); } if (m_lightMeshesDrawing) { Renderer::Enable(RendererParameter_DepthBuffer, true); Renderer::Enable(RendererParameter_DepthWrite, true); Renderer::Enable(RendererParameter_FaceCulling, false); Renderer::Enable(RendererParameter_StencilTest, false); Renderer::SetFaceFilling(FaceFilling_Line); const Shader* shader = ShaderLibrary::Get("DebugSimple"); static int colorLocation = shader->GetUniformLocation("Color"); Renderer::SetShader(shader); for (const auto& light : m_renderQueue->pointLights) { lightMatrix.SetScale(Vector3f(light.radius * 1.1f)); // To correct imperfections due to the sphere lightMatrix.SetTranslation(light.position); Renderer::SetMatrix(MatrixType_World, lightMatrix); shader->SendColor(colorLocation, light.color); Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount()); } Renderer::Enable(RendererParameter_DepthBuffer, false); Renderer::Enable(RendererParameter_DepthWrite, false); Renderer::Enable(RendererParameter_FaceCulling, true); Renderer::Enable(RendererParameter_StencilTest, true); Renderer::SetFaceFilling(FaceFilling_Fill); } } if (!m_renderQueue->spotLights.empty()) { const IndexBuffer* indexBuffer = m_coneMesh->GetIndexBuffer(); Renderer::SetIndexBuffer(indexBuffer); Renderer::SetVertexBuffer(m_coneMesh->GetVertexBuffer()); m_pointSpotLightShader->SendInteger(m_pointSpotLightUniforms.locations.type, LightType_Spot); for (const auto& light : m_renderQueue->spotLights) { m_pointSpotLightShader->SendColor(m_pointSpotLightUniforms.locations.color, light.color); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.factors, Vector2f(light.ambientFactor, light.diffuseFactor)); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters1, Vector4f(light.position, light.attenuation)); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters2, Vector4f(light.direction, light.invRadius)); m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters3, Vector2f(light.innerAngleCosine, light.outerAngleCosine)); float baseRadius = light.radius * light.outerAngleTangent * 1.1f; lightMatrix.MakeTransform(light.position, Quaternionf::RotationBetween(Vector3f::Forward(), light.direction), Vector3f(baseRadius, baseRadius, light.radius)); Renderer::SetMatrix(MatrixType_World, lightMatrix); // Sphere rendering in the stencil buffer Renderer::Enable(RendererParameter_ColorWrite, false); Renderer::Enable(RendererParameter_DepthBuffer, true); Renderer::Enable(RendererParameter_FaceCulling, false); Renderer::SetStencilCompareFunction(RendererComparison_Always); m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, true); Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount()); // Sphere rendering as effect zone Renderer::Enable(RendererParameter_ColorWrite, true); Renderer::Enable(RendererParameter_DepthBuffer, false); Renderer::Enable(RendererParameter_FaceCulling, true); Renderer::SetFaceCulling(FaceSide_Front); Renderer::SetStencilCompareFunction(RendererComparison_NotEqual, FaceSide_Back); Renderer::SetStencilPassOperation(StencilOperation_Zero, FaceSide_Back); m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, false); Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount()); } if (m_lightMeshesDrawing) { Renderer::Enable(RendererParameter_DepthBuffer, true); Renderer::Enable(RendererParameter_DepthWrite, true); Renderer::Enable(RendererParameter_FaceCulling, false); Renderer::Enable(RendererParameter_StencilTest, false); Renderer::SetFaceFilling(FaceFilling_Line); const Shader* shader = ShaderLibrary::Get("DebugSimple"); static int colorLocation = shader->GetUniformLocation("Color"); Renderer::SetShader(shader); for (const auto& light : m_renderQueue->spotLights) { float baseRadius = light.radius * light.outerAngleTangent * 1.1f; lightMatrix.MakeTransform(light.position, Quaternionf::RotationBetween(Vector3f::Forward(), light.direction), Vector3f(baseRadius, baseRadius, light.radius)); Renderer::SetMatrix(MatrixType_World, lightMatrix); shader->SendColor(colorLocation, light.color); Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount()); } Renderer::Enable(RendererParameter_DepthBuffer, false); Renderer::Enable(RendererParameter_DepthWrite, false); Renderer::Enable(RendererParameter_FaceCulling, true); Renderer::Enable(RendererParameter_StencilTest, true); Renderer::SetFaceFilling(FaceFilling_Fill); } } Renderer::Enable(RendererParameter_StencilTest, false); } return true; }
Mesh MeshLoader::uploadMesh(const aiMesh *mesh) { Mesh model; // Generate vertex array object GLuint vao; glGenVertexArrays(1, &vao); glBindVertexArray(vao); // Create vertex indices VBO { // Store face indices in an array std::unique_ptr<unsigned int[]> faceArray(new unsigned int[mesh->mNumFaces * 3]); for (unsigned int f = 0; f < mesh->mNumFaces; ++f) { const aiFace &face = mesh->mFaces[f]; std::memcpy(&faceArray[f * 3], face.mIndices, 3 * sizeof(face.mIndices[0])); } // Prepare vertex indices VBO createVBO(GL_ELEMENT_ARRAY_BUFFER, sizeof(unsigned int) * mesh->mNumFaces * 3, faceArray.get()); } // Cache vertex attributes existence to avoid dereferencing and calling every // time bool hasPositions = mesh->HasPositions(), hasTexCoords = mesh->HasTextureCoords(0), hasNormals = mesh->HasNormals(), hasTangents = mesh->HasTangentsAndBitangents(); // Use them to determine the vertex definition size unsigned int vtxSize = +(hasPositions ? sizeof(float) * 3 : 0) + (hasTexCoords ? sizeof(float) * 2 : 0) + (hasNormals ? sizeof(float) * 3 : 0) + (hasTangents ? sizeof(float) * 3 : 0); // Create a buffer to store vertex data, with determined size since we now // know it TightDataPacker data(mesh->mNumVertices * vtxSize); if (hasPositions) { // Since we keep the vertices in the model, better determine the storage // size once and for all model.vertices.resize(mesh->mNumVertices); } // Process each vertex and store its data for (unsigned int i = 0; i < mesh->mNumVertices; ++i) { if (hasPositions) { const aiVector3D &v = mesh->mVertices[i]; model.vertices[i] = Vector3f(v.x, v.y, v.z); data << v.x << v.y << v.z; } if (hasTexCoords) { const aiVector3D &t = mesh->mTextureCoords[0][i]; data << t.x << 1.f - t.y; // Y must be flipped due to OpenGL's coordinate system } if (hasNormals) { const aiVector3D &n = mesh->mNormals[i]; data << n.x << n.y << n.z; } if (hasTangents) { const aiVector3D &t = mesh->mTangents[i]; data << t.x << t.y << t.z; } } // Describe the vertex format we have { intptr_t offset = 0; specVector vertexAttrib; if (hasPositions) { vertexAttrib.push_back({0, 3, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)}); offset += sizeof(float) * 3; } if (hasTexCoords) { vertexAttrib.push_back({1, 2, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)}); offset += sizeof(float) * 2; } if (hasNormals) { vertexAttrib.push_back({2, 3, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)}); offset += sizeof(float) * 3; } if (hasTangents) { vertexAttrib.push_back({3, 3, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)}); } createVBO(GL_ARRAY_BUFFER, data.getSize(), data.getDataPtr(), static_cast<int>(vtxSize), vertexAttrib); } glBindVertexArray(0); // Store relevant data in the new mesh model.handle = static_cast<int>(vao); model.numFaces = static_cast<int>(mesh->mNumFaces); return model; }
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command switch(packet.command) { case MAV_CMD_START_RX_PAIR: // initiate bind procedure if (!hal.rcin->rc_bind(packet.param1)) { result = MAV_RESULT_FAILED; } else { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(RTL); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif #if CAMERA == ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: rover.camera.configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_DO_DIGICAM_CONTROL: if (rover.camera.control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6)) { rover.log_picture(); } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_CALIBRATION: if(hal.util->get_soft_armed()) { result = MAV_RESULT_FAILED; break; } if (is_equal(packet.param1,1.0f)) { rover.ins.init_gyro(); if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3,1.0f)) { rover.init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param4,1.0f)) { rover.trim_radio(); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param5,1.0f)) { result = MAV_RESULT_ACCEPTED; // start with gyro calibration rover.ins.init_gyro(); // reset ahrs gyro bias if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); } else { result = MAV_RESULT_FAILED; } rover.ins.acal_init(); rover.ins.get_acal()->start(this); } else if (is_equal(packet.param5,2.0f)) { // start with gyro calibration rover.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if(rover.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: { uint8_t compassNumber = -1; if (is_equal(packet.param1, 2.0f)) { compassNumber = 0; } else if (is_equal(packet.param1, 5.0f)) { compassNumber = 1; } else if (is_equal(packet.param1, 6.0f)) { compassNumber = 2; } if (compassNumber != (uint8_t) -1) { rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: rover.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: rover.set_mode(LEARNING); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_SERVO: if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_SET_RELAY: if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_RELAY: if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1,1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_GET_HOME_POSITION: if (rover.home_is_set != HOME_UNSET) { send_home(rover.ahrs.get_home()); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { send_autopilot_version(FIRMWARE_VERSION); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1,1.0f)) { rover.init_home(); } else { if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { // don't allow the 0,0 position break; } // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc {}; new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); rover.ahrs.set_home(new_home_loc); rover.home_is_set = HOME_SET_NOT_LOCKED; rover.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); result = MAV_RESULT_ACCEPTED; rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat*1.0e-7f), (double)(new_home_loc.lng*1.0e-7f), (uint32_t)(new_home_loc.alt*0.01f)); } break; } case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: result = rover.compass.handle_mag_cal_command(packet); break; default: break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t)); break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { handle_mission_request_list(rover.mission, msg); break; } // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST_INT: case MAVLINK_MSG_ID_MISSION_REQUEST: { handle_mission_request(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_ACK: { // not used break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // mark the firmware version in the tlog send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); #endif handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { handle_param_request_read(msg); break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { handle_mission_clear_all(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { handle_mission_set_current(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { handle_mission_count(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { handle_mission_write_partial_list(rover.mission, msg); break; } // GCS has sent us a mission item, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_MISSION_ITEM_INT: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, &rover.DataFlash); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if(msg->sysid != rover.g.sysid_my_gcs) break; // Only accept control from our gcs mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; v[3] = packet.chan4_raw; v[4] = packet.chan5_raw; v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; hal.rcin->set_overrides(v, 8); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != rover.g.sysid_my_gcs) break; rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_GPS_INPUT: { rover.gps.handle_msg(msg); break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // set gps hil sensor Location loc; loc.lat = packet.lat; loc.lng = packet.lon; loc.alt = packet.alt/10; Vector3f vel(packet.vx, packet.vy, packet.vz); vel *= 0.01f; gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, packet.time_usec/1000, loc, vel, 10, 0); // rad/sec Vector3f gyros; gyros.x = packet.rollspeed; gyros.y = packet.pitchspeed; gyros.z = packet.yawspeed; // m/s/s Vector3f accels; accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); ins.set_gyro(0, gyros); ins.set_accel(0, accels); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } #endif // HIL_MODE #if CAMERA == ENABLED //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { break; } //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONTROL: { rover.camera.control_msg(msg); rover.log_picture(); break; } #endif // CAMERA == ENABLED #if MOUNT == ENABLED //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } case MAVLINK_MSG_ID_LOG_REQUEST_DATA: case MAVLINK_MSG_ID_LOG_ERASE: rover.in_log_download = true; /* no break */ case MAVLINK_MSG_ID_LOG_REQUEST_LIST: if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_LOG_REQUEST_END: rover.in_log_download = false; if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, rover.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, rover.gps); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.sonar.handle_msg(msg); break; case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: rover.DataFlash.remote_log_block_status_msg(chan, msg); break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: send_autopilot_version(FIRMWARE_VERSION); break; case MAVLINK_MSG_ID_SETUP_SIGNING: handle_setup_signing(msg); break; case MAVLINK_MSG_ID_LED_CONTROL: // send message to Notify AP_Notify::handle_led_control(msg); break; case MAVLINK_MSG_ID_PLAY_TUNE: // send message to Notify AP_Notify::handle_play_tune(msg); break; } // end switch } // end handle mavlink
/* this offset nulling algorithm is inspired by this paper from Bill Premerlani http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf The base algorithm works well, but is quite sensitive to noise. After long discussions with Bill, the following changes were made: 1) we keep a history buffer that effectively divides the mag vectors into a set of N streams. The algorithm is run on the streams separately 2) within each stream we only calculate a change when the mag vector has changed by a significant amount. This gives us the property that we learn quickly if there is no noise, but still learn correctly (and slowly) in the face of lots of noise. */ void Compass::null_offsets(void) { if (_null_enable == false || _learn == 0) { // auto-calibration is disabled return; } // this gain is set so we converge on the offsets in about 5 // minutes with a 10Hz compass const float gain = 0.01; const float max_change = 10.0; const float min_diff = 50.0; Vector3f ofs; ofs = _offset.get(); if (!_null_init_done) { // first time through _null_init_done = true; for (uint8_t i=0; i<_mag_history_size; i++) { // fill the history buffer with the current mag vector, // with the offset removed _mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); } _mag_history_index = 0; return; } Vector3f b1, b2, diff; float length; // get a past element b1 = Vector3f(_mag_history[_mag_history_index].x, _mag_history[_mag_history_index].y, _mag_history[_mag_history_index].z); // the history buffer doesn't have the offsets b1 += ofs; // get the current vector b2 = Vector3f(mag_x, mag_y, mag_z); // calculate the delta for this sample diff = b2 - b1; length = diff.length(); if (length < min_diff) { // the mag vector hasn't changed enough - we don't get // enough information from this vector to use it. // Note that we don't put the current vector into the mag // history here. We want to wait for a larger rotation to // build up before calculating an offset change, as accuracy // of the offset change is highly dependent on the size of the // rotation. _mag_history_index = (_mag_history_index + 1) % _mag_history_size; return; } // put the vector in the history _mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z); _mag_history_index = (_mag_history_index + 1) % _mag_history_size; // equation 6 of Bills paper diff = diff * (gain * (b2.length() - b1.length()) / length); // limit the change from any one reading. This is to prevent // single crazy readings from throwing off the offsets for a long // time length = diff.length(); if (length > max_change) { diff *= max_change / length; } // set the new offsets _offset.set(_offset.get() - diff); }
bool LogReader::update(uint8_t &type) { uint8_t hdr[3]; if (::read(fd, hdr, 3) != 3) { return false; } if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) { printf("bad log header\n"); return false; } if (hdr[2] == LOG_FORMAT_MSG) { struct log_Format &f = formats[num_formats]; memcpy(&f, hdr, 3); if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) { return false; } num_formats++; type = f.type; return true; } uint8_t i; for (i=0; i<num_formats; i++) { if (formats[i].type == hdr[2]) break; } if (i == num_formats) { return false; } const struct log_Format &f = formats[i]; uint8_t data[f.length]; memcpy(data, hdr, 3); if (::read(fd, &data[3], f.length-3) != f.length-3) { return false; } switch (f.type) { case LOG_MESSAGE_MSG: { struct log_Message msg; if(sizeof(msg) != f.length) { printf("Bad MESSAGE length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) { vehicle = VEHICLE_PLANE; ::printf("Detected Plane\n"); } else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) { vehicle = VEHICLE_COPTER; ::printf("Detected Copter\n"); } else if (strncmp(msg.msg, "APMRover2", strlen("APMRover2")) == 0) { vehicle = VEHICLE_ROVER; ::printf("Detected Rover\n"); } break; } case LOG_IMU_MSG: { struct log_IMU msg; if(sizeof(msg) != f.length) { printf("Bad IMU length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.timestamp); if (gyro_mask & 1) { ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z)); } if (accel_mask & 1) { ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z)); } break; } case LOG_IMU2_MSG: { struct log_IMU msg; if(sizeof(msg) != f.length) { printf("Bad IMU2 length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.timestamp); if (gyro_mask & 2) { ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z)); } if (accel_mask & 2) { ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z)); } break; } case LOG_GPS_MSG: { struct log_GPS msg; if(sizeof(msg) != f.length) { printf("Bad GPS length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.apm_time); gps->setHIL(msg.status==3?GPS::FIX_3D:GPS::FIX_NONE, msg.apm_time, msg.latitude*1.0e-7f, msg.longitude*1.0e-7f, msg.altitude*1.0e-2f, msg.ground_speed*1.0e-2f, msg.ground_course*1.0e-2f, 0, msg.num_sats); if (msg.status == 3 && ground_alt_cm == 0) { ground_alt_cm = msg.altitude; } rel_altitude = msg.rel_altitude*0.01f; break; } case LOG_SIMSTATE_MSG: { struct log_AHRS msg; if(sizeof(msg) != f.length) { printf("Bad SIMSTATE length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f); break; } case LOG_BARO_MSG: { struct log_BARO msg; if(sizeof(msg) != f.length) { printf("Bad LOG_BARO length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.timestamp); baro.setHIL(msg.pressure, msg.temperature*0.01f); break; } case LOG_PARAMETER_MSG: { struct log_Parameter msg; if(sizeof(msg) != f.length) { printf("Bad LOG_PARAMETER length\n"); exit(1); } memcpy(&msg, data, sizeof(msg)); set_parameter(msg.name, msg.value); break; } default: if (vehicle == VEHICLE_PLANE) { process_plane(f.type, data, f.length); } else if (vehicle == VEHICLE_COPTER) { process_copter(f.type, data, f.length); } break; } type = f.type; return true; }
void pbrtTranslate(Float dx, Float dy, Float dz) { VERIFY_INITIALIZED("Translate"); FOR_ACTIVE_TRANSFORMS(curTransform[i] = curTransform[i] * Translate(Vector3f(dx, dy, dz));) }
Vector3f Enclosure::move(const Vector3f& fromPos, const Vector3f& direction, float width, float height) { // first, obtain motion distance float distance = direction.length(); // we should determine maximum distance that we can move safely, without probability of // leave enclosure boundary float safeDistance = 0.5f * ( width < height ? width : height ); // now prepare to move Vector3f pos = fromPos; Vector3f dir = direction; dir.normalize(); Vector3f penetration; float stepDistance; unsigned int i; // move until there is a distance left do { // determine step distance stepDistance = safeDistance < distance ? safeDistance : distance; // move along normalized direction pos += dir * stepDistance; // detect "foot" collision _numTriangles = 0; _ray->setRay( pos, Vector3f( 0,-1,0 ) * height ); _ray->intersect( _collisionAtomic, onRayCollision, this ); if( _numTriangles ) { // determine penetration vector penetration = _nearestTriangle.normal * height * ( 1.0f - _nearestTriangle.distance ); // disable penetration pos += penetration; } // detect "body" collision for( i=0; i<_wallNormals.size(); i++ ) { _numTriangles = 0; _ray->setRay( pos, _wallNormals[i] * -width ); _ray->intersect( _collisionAtomic, onRayCollision, this ); if( _numTriangles ) { // determine penetration vector penetration = _nearestTriangle.normal * width * ( 1.0f - _nearestTriangle.distance ); // disable penetration pos += penetration; } } // decrease distance left distance -= stepDistance; } while( distance > 0 ); return pos; }
void pbrtLookAt(Float ex, Float ey, Float ez, Float lx, Float ly, Float lz, Float ux, Float uy, Float uz) { VERIFY_INITIALIZED("LookAt"); Transform lookAt = LookAt(Point3f(ex, ey, ez), Point3f(lx, ly, lz), Vector3f(ux, uy, uz)); FOR_ACTIVE_TRANSFORMS(curTransform[i] = curTransform[i] * lookAt;);
/// sets the servo angles for retraction, note angles are in degrees void AP_Mount::set_retract_angles(float roll, float tilt, float pan) { _retract_angles = Vector3f(roll, tilt, pan); }