C++ Vector3f函数代码示例

/// 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);
//.........这里部分代码省略.........

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() );
    }
    */
}