C++ Vec3f类代码示例

void Galaxy::renderGalaxyPointSprites(const GLContext&,
                                      const Vec3f& offset,
                                      const Quatf& viewerOrientation,
                                      float brightness,
                                      float pixelSize)
{
    if (form == NULL)
        return;

    /* We'll first see if the galaxy's apparent size is big enough to
       be noticeable on screen; if it's not we'll break right here,
       avoiding all the overhead of the matrix transformations and
       GL state changes: */
        float distanceToDSO = offset.length() - getRadius();
        if (distanceToDSO < 0)
            distanceToDSO = 0;

        float minimumFeatureSize = pixelSize * distanceToDSO;
        float size  = 2 * getRadius();

        if (size < minimumFeatureSize)
            return;

    if (galaxyTex == NULL)
    {
        galaxyTex = CreateProceduralTexture(width, height, GL_RGBA,
                                            GalaxyTextureEval);
    }
    assert(galaxyTex != NULL);

    glEnable(GL_TEXTURE_2D);
    galaxyTex->bind();

    Mat3f viewMat = viewerOrientation.toMatrix3();
    Vec3f v0 = Vec3f(-1, -1, 0) * viewMat;
    Vec3f v1 = Vec3f( 1, -1, 0) * viewMat;
    Vec3f v2 = Vec3f( 1,  1, 0) * viewMat;
    Vec3f v3 = Vec3f(-1,  1, 0) * viewMat;

    //Mat4f m = (getOrientation().toMatrix4() *
    //           Mat4f::scaling(form->scale) *
    //           Mat4f::scaling(getRadius()));

    Mat3f m =
        Mat3f::scaling(form->scale)*getOrientation().toMatrix3()*Mat3f::scaling(size);

    // Note: fixed missing factor of 2 in getRadius() scaling of galaxy diameter!
    // Note: fixed correct ordering of (non-commuting) operations!

    int   pow2  = 1;

    vector<Blob>* points = form->blobs;
    unsigned int nPoints = (unsigned int) (points->size() * clamp(getDetail()));
    // corrections to avoid excessive brightening if viewed e.g. edge-on

    float brightness_corr = 1.0f;
    float cosi;

    if (type < E0 || type > E3) //all galaxies, except ~round elliptics
    {
        cosi = Vec3f(0,1,0) * getOrientation().toMatrix3()
                            * offset/offset.length();
        brightness_corr = (float) sqrt(abs(cosi));
        if (brightness_corr < 0.2f)
            brightness_corr = 0.2f;
    }
    if (type > E3) // only elliptics with higher ellipticities
    {
        cosi = Vec3f(1,0,0) * getOrientation().toMatrix3()
                            * offset/offset.length();
        brightness_corr = brightness_corr * (float) abs((cosi));
        if (brightness_corr < 0.45f)
            brightness_corr = 0.45f;
    }

    glBegin(GL_QUADS);
    for (unsigned int i = 0; i < nPoints; ++i)
    {
        if ((i & pow2) != 0)
        {
            pow2 <<= 1;
            size /= 1.55f;
            if (size < minimumFeatureSize)
                break;
        }

        Blob    b  = (*points)[i];
        Point3f p  = b.position * m;
        float   br = b.brightness / 255.0f;

        Color   c      = colorTable[b.colorIndex];     // lookup static color table
        Point3f relPos = p + offset;

        float screenFrac = size / relPos.distanceFromOrigin();
        if (screenFrac < 0.1f)
        {
            float btot = ((type > SBc) && (type < Irr))? 2.5f: 5.0f;
            float a  = btot * (0.1f - screenFrac) * brightness_corr * brightness * br;

            glColor4f(c.red(), c.green(), c.blue(), (4.0f*lightGain + 1.0f)*a);
//.........这里部分代码省略.........

void Camera::setViewDirection( const Vec3f &aViewDirection )
{
	mViewDirection = aViewDirection.normalized();
	mOrientation = Quatf( Vec3f( 0.0f, 0.0f, -1.0f ), mViewDirection );
	mModelViewCached = false;
}

	//Computes the normals, if they haven't been computed yet
	void computeNormals() {
		if (computedNormals) {
			return;
		}

		//Compute the rough version of the normals
		Vec3f** normals2 = new Vec3f*[l];
		for (int i = 0; i < l; i++) {
			normals2[i] = new Vec3f[w];
		}

		for (int z = 0; z < l; z++) {
			for (int x = 0; x < w; x++) {
				Vec3f sum(0.0f, 0.0f, 0.0f);

				Vec3f out;
				if (z > 0) {
					out = Vec3f(0.0f, hs[z - 1][x] - hs[z][x], -1.0f);
				}
				Vec3f in;
				if (z < l - 1) {
					in = Vec3f(0.0f, hs[z + 1][x] - hs[z][x], 1.0f);
				}
				Vec3f left;
				if (x > 0) {
					left = Vec3f(-1.0f, hs[z][x - 1] - hs[z][x], 0.0f);
				}
				Vec3f right;
				if (x < w - 1) {
					right = Vec3f(1.0f, hs[z][x + 1] - hs[z][x], 0.0f);
				}

				if (x > 0 && z > 0) {
					sum += out.cross(left).normalize();
				}
				if (x > 0 && z < l - 1) {
					sum += left.cross(in).normalize();
				}
				if (x < w - 1 && z < l - 1) {
					sum += in.cross(right).normalize();
				}
				if (x < w - 1 && z > 0) {
					sum += right.cross(out).normalize();
				}

				normals2[z][x] = sum;
			}
		}

		//Smooth out the normals
		const float FALLOUT_RATIO = 0.5f;
		for (int z = 0; z < l; z++) {
			for (int x = 0; x < w; x++) {
				Vec3f sum = normals2[z][x];

				if (x > 0) {
					sum += normals2[z][x - 1] * FALLOUT_RATIO;
				}
				if (x < w - 1) {
					sum += normals2[z][x + 1] * FALLOUT_RATIO;
				}
				if (z > 0) {
					sum += normals2[z - 1][x] * FALLOUT_RATIO;
				}
				if (z < l - 1) {
					sum += normals2[z + 1][x] * FALLOUT_RATIO;
				}

				if (sum.magnitude() == 0) {
					sum = Vec3f(0.0f, 1.0f, 0.0f);
				}
				normals[z][x] = sum;
			}
		}

		for (int i = 0; i < l; i++) {
			delete[] normals2[i];
		}
		delete[] normals2;

		computedNormals = true;
	}

void VRShadowEngine::setupCamera(Light         *pLight,
                                        LightTypeE     eType,
                                        RenderAction  *pAction,
                                        EngineDataPtr  pEngineData)
{
    if(eType == Directional)
    {
        DirectionalLight *pDLight =
            dynamic_cast<DirectionalLight *>(pLight);

        MatrixCameraUnrecPtr pCam =
            dynamic_cast<MatrixCamera *>(pEngineData->getCamera());

        if(pCam == NULL)
        {
            pCam = MatrixCamera::createLocal();

            pEngineData->setCamera(pCam);
        }


        Vec3f   diff;
        Pnt3f   center;
        Matrix  transMatrix;
        Node   *pNode = pAction->getActNode();

//        tmpDir = DirectionalLightPtr::dcast(_lights[i]);

        diff = (pNode->getVolume().getMax() -
                pNode->getVolume().getMin());

        Real32 sceneWidth = diff.length() * 0.5f;
        // Not final values. May get tweaked in the future

        Real32 sceneHeight = diff.length() * 0.5f;

        pNode->getVolume().getCenter(center);

        Vec3f lightdir = pDLight->getDirection();

        if(pLight->getBeacon() != NULL)
        {
            Matrix m = pLight->getBeacon()->getToWorld();

            m.mult(lightdir, lightdir);
        }

        MatrixLookAt(transMatrix,
                     center + lightdir,
                     center,
                     Vec3f(0,1,0));

        transMatrix.invert();

        Matrix proMatrix;

        proMatrix.setIdentity();

        MatrixOrthogonal( proMatrix,
                         -sceneWidth,   sceneWidth, -sceneHeight,
                          sceneHeight, -sceneWidth,  sceneWidth);


        pCam->setProjectionMatrix(proMatrix  );
        pCam->setModelviewMatrix (transMatrix);
    }
    else if(eType == Point)
    {
        PointLight *pPLight = dynamic_cast<PointLight *>(pLight);

        MatrixCameraUnrecPtr pCam =
            dynamic_cast<MatrixCamera *>(pEngineData->getCamera());

        if(pCam == NULL)
        {
            pCam = MatrixCamera::createLocal();

            pEngineData->setCamera(pCam);
        }

        Real32  angle;
        Vec3f   dist;
        Pnt3f   center;
        Vec3f   diff;

        Matrix  transMatrix;

        Node   *pNode = pAction->getActNode();


        pNode->getVolume().getCenter(center);

        Pnt3f lightpos = pPLight->getPosition();

        if(pLight->getBeacon() != NULL)
        {
            Matrix m = pLight->getBeacon()->getToWorld();

            m.mult(lightpos, lightpos);
        }
//.........这里部分代码省略.........

void Tube::buildFrenet()
{
	mFrames.clear();
	
	int n = mPs.size();
	mFrames.resize( n );
	
	for( int i = 0; i < n; ++i ) {
		Vec3f p0, p1, p2;		
		if( i < (n - 2) ) {
			p0 = mPs[i];
			p1 = mPs[i + 1];
			p2 = mPs[i + 2];
		}
		else if( i == (n - 2) ) {
			p0 = mPs[i - 1];
			p1 = mPs[i];
			p2 = mPs[i + 1];
		}	
		else if( i == (n - 1) ) {
			p0 = mPs[i - 3];
			p1 = mPs[i - 2];
			p2 = mPs[i - 1];
		}

		
	    Vec3f t = (p1 - p0).normalized();
		Vec3f n = t.cross(p2 - p0).normalized();
		if( n.length() == 0.0f ) {
			int i = fabs( t[0] ) < fabs( t[1] ) ? 0 : 1;
			if( fabs( t[2] ) < fabs( t[i] ) ) 
				i = 2;
				
			Vec3f v( 0.0f, 0.0f, 0.0f ); 
			v[i] = 1.0;
			n = t.cross( v ).normalized();
		}
		Vec3f b = t.cross( n );	
	
		Matrix44f& m = mFrames[i];
		m.at( 0, 0 ) = b.x;
		m.at( 1, 0 ) = b.y;
		m.at( 2, 0 ) = b.z;
		m.at( 3, 0 ) = 0;
		
		m.at( 0, 1 ) = n.x;
		m.at( 1, 1 ) = n.y;
		m.at( 2, 1 ) = n.z;
		m.at( 3, 1 ) = 0;
		
		m.at( 0, 2 ) = t.x;
		m.at( 1, 2 ) = t.y;
		m.at( 2, 2 ) = t.z;
		m.at( 3, 2 ) = 0;
		
		m.at( 0, 3 ) = mPs[i].x;
		m.at( 1, 3 ) = mPs[i].y;
		m.at( 2, 3 ) = mPs[i].z;
		m.at( 3, 3 ) = 1;
	}
}

void CPlanetFinderEngine::buildSolarSystemList( std::vector< star3map::Sprite > & solarsystem )
{
	solarsystem.clear();
	
	float moonN0 = 125.1228;
	float moonw0 = 318.0634;
	//float moonM0 = 115.3654;
	float moonN = 	moonN0 - 0.0529538083 * daysSince2000;
	float moonw =  moonw0 + 0.1643573223 * daysSince2000;
	//float MoonM = 	moonM0 + 13.0649929509 * daysSince2000;

	moon.Init("Moon",1.23e-02,27.322,2.569519e-03,0.0549,5.145,
	     		moonN,
	     		moonN+moonw, //-- lon of peri = N + w
	     		/*moonN0+moonw0+moonM0*/ // meanlong2000 = N+w+M
	     		218.32,	//use Dave's data instead
	     		false,0.,0.,0.,0.);

	// This is the correction for parallax due to the earth's rotation.
	Vec3f earthPosition = earth.position( daysSince2000, 0.000001 ); // + (zenith * (float)(4.3e-05));
	Vec3f earthToMoon = moon.position( daysSince2000, 0.000001 );
	//--- Nonkeplerian perturbations for the moon:
	//First convert vector to spherical coords:
	float moonRad = earthToMoon.Length();
	float moonLat = acos( -earthToMoon.z / moonRad ) - M_PI/2.0;
	float moonLon = atan2(earthToMoon.y, earthToMoon.x);

	if ( moonLon < -M_PI/2.0 ) moonLon += M_PI;
	if ( moonLon > M_PI/2.0 ) moonLon -= M_PI;
	if ( earthToMoon.x < 0.0 ) moonLon += M_PI;
	if ( moonLon < 0.0 ) moonLon += 2*M_PI;
	if ( moonLon > 2.0*M_PI ) moonLon -= 2*M_PI;

	moonLon = moonLon + MoonPerturbations::moonLongitudeCorrectionDegrees(daysSince2000)*M_PI/180.0;
	moonLat = moonLat + MoonPerturbations::moonLatitudeCorrectionDegrees(daysSince2000)*M_PI/180.0;
		
	earthToMoon = latLongToUnitVector(moonLat,moonLon);
	earthToMoon *= moonRad;

	Vec3f moonPosition = earthToMoon + earthPosition;

	
	//================================================================================
	//		Show planets, moon & sun
	//================================================================================
	Vec3f planetsCenterOfMass(0, 0 , 0);
	Vec3f currentPosition[9];
	int i;

	for (i= 0; i<PLANETS_NUMBER; i++) 
	{
		currentPosition[i] = planets[i]->position( daysSince2000, 0.000001 );
		planetsCenterOfMass += currentPosition[i] * planets[i]->mass;
	}
	
	Vec3f sunPosition =  planetsCenterOfMass*(float)(-1.0/sunMass);
	for (i= -1; i<PLANETS_NUMBER; i++) 
	{   // Yuck.
		//==moon is when you do earth, sun is i== -1
		Vec3f directionFromEarth;
		SetColor( Vec4f( 1, 1, 1 ) );
		Texture2D *tex = NULL;
		std::string name;
		bool isSun,isMoon;
		isSun=false;
		isMoon=false;
		float scale = 1.0;
			
		if (i!= -1 && planets[i]!=&earth) 
		{
			// a planet other than earth
			directionFromEarth = currentPosition[i] - earthPosition;
			directionFromEarth.Normalize();
			tex = planets[ i ]->texture;
			name = planets[i]->name;
			scale = planets[ i ]->scale;
		}
		else 
		{
			if (i== -1) 
			{
				//-- sun
				directionFromEarth = sunPosition - earthPosition;
				directionFromEarth.Normalize();
				tex = sunTexture;
				name = "Sun";
				isSun = true;
			}
			else 
			{
				//-- moon
				directionFromEarth = moonPosition - earthPosition;
				directionFromEarth.Normalize();
				tex = moon.texture;
				name = "Moon";
				isMoon=true;
			}
		}
		
		int dotRadius = (isSun || isMoon) ? 5 : 2;
//.........这里部分代码省略.........

void GLWidget::drawBoundingBox()
{
    Vec3f volumeScale = Vec3f(volumeDim);
    volumeScale = volumeScale / volumeScale.norm();
    std::vector<Vec3f> BoundingBoxPlane1;
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, -volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, -volumeScale.z));
//    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, -volumeScale.z));
//    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, -volumeScale.z));
//    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, -volumeScale.z));
//    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, -volumeScale.z));
    GLArrayBuffer::Ptr boundingBoxArrayBuffer = GLArrayBuffer::create(GL_ARRAY_BUFFER);
    boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
    GLVertexArray::Ptr bBoxVertexArray = GLVertexArray::create();
    volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);


    boundBoxProgram->setUniform("projectM", projectionMatrix);
    boundBoxProgram->setUniform("modelview", modelViewMatrix);
    boundBoxProgram->begin();
    bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
    boundBoxProgram->end();

    BoundingBoxPlane1.clear();
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, -volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, -volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, -volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, -volumeScale.z));
    boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
    volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);


    boundBoxProgram->setUniform("projectM", projectionMatrix);
    boundBoxProgram->setUniform("modelview", modelViewMatrix);
    boundBoxProgram->begin();
    bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
    boundBoxProgram->end();

    BoundingBoxPlane1.clear();
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, -volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, -volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, volumeScale.z));
    boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
    volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);


    boundBoxProgram->setUniform("projectM", projectionMatrix);
    boundBoxProgram->setUniform("modelview", modelViewMatrix);
    boundBoxProgram->begin();
    bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
    boundBoxProgram->end();

    BoundingBoxPlane1.clear();
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, volumeScale.y, volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(-volumeScale.x, -volumeScale.y, volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, -volumeScale.y, volumeScale.z));
    BoundingBoxPlane1.push_back(Vec3f(volumeScale.x, volumeScale.y, volumeScale.z));
    boundingBoxArrayBuffer->update(BoundingBoxPlane1.size() * sizeof(Vec3f), &BoundingBoxPlane1.front(), GL_STATIC_DRAW);
    volumeRayCastingProgram->setVertexAttribute("Vertex", bBoxVertexArray, boundingBoxArrayBuffer, 3, GL_FLOAT, false);


    boundBoxProgram->setUniform("projectM", projectionMatrix);
    boundBoxProgram->setUniform("modelview", modelViewMatrix);
    boundBoxProgram->begin();
    bBoxVertexArray->drawArrays(GL_LINE_LOOP, 4);
    boundBoxProgram->end();
}

uint32_t GatherForestBuilder::_SplitPoints(vector<GatherNode> &tree, vector<GatherPoint> &gps, vector<GatherPoint>::iterator start, vector<GatherPoint>::iterator end )
{
    Range3f pBox(Vec3f(FLT_MAX, FLT_MAX, FLT_MAX), Vec3f(-FLT_MAX, -FLT_MAX, -FLT_MAX));
    Range3f nBox(Vec3f(FLT_MAX, FLT_MAX, FLT_MAX), Vec3f(-FLT_MAX, -FLT_MAX, -FLT_MAX));
    Conef   cone;

    vector<GatherPoint>::iterator it = start;
    while (it != end)
    {
        pBox.Grow(it->position);
        nBox.Grow(it->normal);
        if (it == start)
            cone = Conef(it->normal);
        else
            cone.Grow(it->normal);
        it++;
    }

    GatherNode node(_tsample);
    node.bbox = pBox;
    node.cone = cone;
    tree.push_back(node);
    uint32_t curIdx = tree.size() - 1;

#ifdef _DEBUG
    vector<GatherPoint> gp(start, end);
#endif // _DEBUG
    if (end - start > 2)
    {
        Vec3f nbSize = nBox.GetSize();
        Vec3f pbSize = pBox.GetSize();

        bool splitType;
        uint32_t splitDim;
        if (pbSize.MaxComponent() > nbSize.MaxComponent())
        {
            splitType = true;
            splitDim = pbSize.MaxComponentIndex();
        }
        else
        {
            splitType = false;
            splitDim = nbSize.MaxComponentIndex();
        }
        vector<GatherPoint>::iterator splitPos = start + (end - start) / 2;
        std::nth_element(start, splitPos, end, CompareNode<GatherPoint>(splitType, splitDim));
#ifdef _DEBUG
        vector<GatherPoint> gp1(start, splitPos);
        vector<GatherPoint> gp2(splitPos, end);
#endif // _DEBUG

        uint32_t leftIdx  = _SplitPoints(tree, gps, start, splitPos);
        uint32_t rightIdx = _SplitPoints(tree, gps, splitPos, end);

        GatherNode &curNode = tree[curIdx];
        curNode.leftIdx = leftIdx;
        curNode.rightIdx = rightIdx;
        vector<uint32_t> &reps = curNode.reps;
        vector<uint32_t> &lreps = tree[leftIdx].reps;
        vector<uint32_t> &rreps = tree[rightIdx].reps;
        
        
        for (uint32_t i = 0; i < reps.size(); i++)
        {
            if (lreps[i] != -1 && rreps[i] != -1)
            {
                //GatherPoint &p1 = gps[lreps[i]];
                //GatherPoint &p2 = gps[reps[i]];
                reps[i] = (__rand() > 0.5f) ? lreps[i] : rreps[i];
            }
            else
            {
                reps[i] = lreps[i] + 1 + rreps[i];
            }
        }
    }
    else
    {
        vector<GatherPoint>::iterator it = start;
        while (it != end)
        {
            gps.push_back(*it);
            tree[curIdx].reps[it->timeInst] = gps.size() - 1;
            it++;
        }
    }
    
    return curIdx;
}

Vec2f EquirectangularCamera::directionToUV(const Vec3f &wi, float &sinTheta) const
{
    Vec3f wLocal = _invRot*wi;
    sinTheta = std::sqrt(max(1.0f - wLocal.y()*wLocal.y(), 0.0f));
    return Vec2f(std::atan2(wLocal.z(), wLocal.x())*INV_TWO_PI + 0.5f, 1.0f - std::acos(clamp(-wLocal.y(), -1.0f, 1.0f))*INV_PI);
}

void Figure::setBallVelocityAfterCollsision( Vec3f* vel,Vec3f center, float radius )
{
	// TODO missing projection to center axis!!!!!!!!!!!!!!
	// !!MUCH better shape dependent collision
	//*vel = *vel * ((mass - this->mMass)/( mass + this->mMass)) + this->mVel * ((this->mMass*2)/( mass + this->mMass));
	
	// get projection on center line (only if the collision occurs on the "side")
	float len = (center - this->mPos).dot(this->mHalfAxe.safeNormalized());
	if( len>(-this->mHalfAxe.length()) && len<this->mHalfAxe.length())
	{
		Vec3f ray = (this->mPos + this->mHalfAxe.safeNormalized()*len - center);
		float penalty =ray.length();
		float scale=((radius + this->mRadius)/penalty);//*((radius + this->mRadius)/penalty);
		ray = ray.safeNormalized();
		float l=ray.dot(this->mVel);
		l = l>0?l:-l;
		*vel = *vel - ray*(ray.dot(*vel))*2 - ray*l*scale;	
	}
	else
	{

		if( len>(this->mHalfAxe.length()) )
		{
			Vec3f ray = this->mHalfAxe.safeNormalized();
			float penalty =(center-this->mPos).length();
			float scale=((radius + this->mHalfAxe.length())/penalty);//*((radius + this->mHalfAxe.length())/penalty);
			*vel = *vel - ray*ray.dot(*vel)*2 + ray*(ray.dot(this->mVel))*scale;
		}
		else
		{
			Vec3f ray = - this->mHalfAxe.safeNormalized();
			float penalty =(center-this->mPos).length();
			float scale=((radius + this->mHalfAxe.length())/penalty);//*((radius + this->mHalfAxe.length())/penalty);
			*vel = *vel - ray*ray.dot(*vel)*2 + ray*ray.dot(this->mVel)*scale;
		}
	}

}

void MyAppli::onExec () {
    if (getClock("RequestTime").getElapsedTime().asSeconds() >= timeBtwnTwoReq.asSeconds()) {
        std::string request = "GETCARPOS";
        sf::Packet packet;
        packet<<request;
        Network::sendUdpPacket(packet);
        getClock("RequestTime").restart();
        received = false;
    }
    std::string response;
    if (Network::getResponse("STOPCARMOVE", response)) {
        std::vector<std::string> infos = split(response, "*");
        int id = conversionStringInt(infos[0]);
        Vec3f newPos (conversionStringFloat(infos[1]), conversionStringFloat(infos[2]), 0);
        Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
        Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
        if (hero->getId() == id) {
            for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
                View view = getRenderComponentManager().getRenderComponent(i)->getView();
                Vec3f d = newPos - view.getPosition();
                view.move(d.x, d.y, d.y);
                getRenderComponentManager().getRenderComponent(i)->setView(view);
            }
            Vec3f d = newPos - getView().getPosition();
            getView().move(d.x, d.y, d.y);
        }
        Vec3f d = newPos - actualPos;
        World::moveEntity(caracter, d.x, d.y, d.y);
        caracter->setMoving(false);
        World::update();
    }
    if (Network::getResponse("MONSTERONMOUSE", response)) {
        std::cout<<"monster on mouse!"<<std::endl;
    }
    if (Network::getResponse("NEWPATH", response)) {
        std::vector<std::string> infos = split(response, "*");
        std::vector<Vec2f> path;
        int size = conversionStringInt(infos[0]);
        int id = conversionStringInt(infos[1]);
        Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
        Vec2f actualPos (conversionStringFloat(infos[2]), conversionStringFloat(infos[3]));
        Vec2f newPos = Computer::getPosOnPathFromTime(actualPos, caracter->getPath(),ping,caracter->getSpeed());
        for (int i = 0; i < size; i++) {
            path.push_back(Vec2f(conversionStringFloat(infos[i*2+4]), conversionStringFloat(infos[i*2+5])));
        }
        Vec2f d = newPos - actualPos;
        Vec2f dir = d.normalize();
        if (dir != caracter->getDir())
            caracter->setDir(dir);
        World::moveEntity(caracter, d.x, d.y, d.y);
        caracter->setPath(path);
        caracter->setMoving(true);
        caracter->interpolation.first = caracter->getCenter();
        caracter->interpolation.second = Computer::getPosOnPathFromTime(caracter->interpolation.first, caracter->getPath(),ping + timeBtwnTwoReq.asMicroseconds(),caracter->getSpeed());
        caracter->getClkTransfertTime().restart();
    }
    if (Network::getResponse("NEWPOS", response)) {
        std::vector<std::string> infos = split(response, "*");
        if (infos.size() == 4) {
            int id = conversionStringInt(infos[0]);
            ping = conversionStringLong(infos[1]);
            Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
            Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
            Vec3f newPos (conversionStringFloat(infos[2]), conversionStringFloat(infos[3]), 0);
            Vec3f d = newPos - actualPos;
            if (id == hero->getId()) {
                for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
                    View view = getRenderComponentManager().getRenderComponent(i)->getView();
                    view.move(d.x, d.y, d.y);
                    getRenderComponentManager().getRenderComponent(i)->setView(view);
                }
                getView().move (d.x, d.y, d.y);
            }
            World::moveEntity(caracter, d.x, d.y, d.y);
            World::update();
            caracter->interpolation.first = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
            if (caracter->isMoving()) {
                if (caracter->isMovingFromKeyboard()) {
                    caracter->interpolation.second = caracter->interpolation.first + Vec3f(caracter->getDir().x,caracter->getDir().y,0)  * caracter->getSpeed() * (ping + timeBtwnTwoReq.asMicroseconds());
                } else {
                    caracter->interpolation.second = Computer::getPosOnPathFromTime(caracter->interpolation.first, caracter->getPath(),ping + timeBtwnTwoReq.asMicroseconds(),caracter->getSpeed());
                }
            } else {
                caracter->interpolation.second = caracter->interpolation.first;
            }
            caracter->getClkTransfertTime().restart();
        }

   } else {
       std::vector<Entity*> caracters = World::getEntities("E_MONSTER+E_HERO");
       for (unsigned int i = 0; i < caracters.size(); i++) {
            Caracter* caracter = static_cast<Caracter*>(caracters[i]);
            if (caracter->isMoving()) {
                if (caracter->isMovingFromKeyboard()) {
                    Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
                    sf::Int64 elapsedTime = caracter->getClkTransfertTime().getElapsedTime().asMicroseconds();
                    Vec3f newPos = caracter->interpolation.first + (caracter->interpolation.second - caracter->interpolation.first) * ((float) elapsedTime / (float) (ping + timeBtwnTwoReq.asMicroseconds()));
                    Ray ray(actualPos, newPos);
                    if (World::collide(caracter, ray)) {
                        newPos = actualPos;
//.........这里部分代码省略.........

void Cone::Parameters(const Vec3f &p, std::pair< float, float > *param) const
{
	// parametrize
	Vec3f s = p - m_center;
	float height = m_axisDir.dot(s);
	float planex = s.dot(m_hcs[0].Data());
	float planey = s.dot(m_hcs[1].Data());
	float l = planex * planex + planey * planey;
	if(l > 0)
	{
		planex /= l;
		planey /= l;
	}
	float angle = std::atan2(planey, planex);
	if(angle < 0)
		angle += float(2 * M_PI);
	/*Vec3f axisDiff = s - height * m_axisDir;
	axisDiff.normalize();
	float angle = m_angular.dot(axisDiff);
	if(angle < -1) // clamp angle to [-1, 1]
		angle = -1;
	else if(angle > 1)
		angle = 1;
	if(m_angular.cross(axisDiff).dot(m_axisDir) < 0)
		angle = std::acos(-angle) + M_PI;
	else
		angle = std::acos(angle);
	// angle ok*/
	// get length from height
	//float length = height / std::cos(m_angle);
	//param->first = length;
	// this should be more precise than a division with std::cos:
	// this is for two sided cone!
	// distance to axis
	float sqrS = s.sqrLength();
	float f = sqrS - (height * height);
	if(f <= 0)
		f = 0;
	else
		f = std::sqrt(f);
	float sdist = fabs(m_n2d[0] * f + ((height < 0)? -1 : 1) * m_n2d[1] * height);
	float length = std::sqrt(sqrS + sdist * sdist);
	param->first = /*(height < 0)? -length :*/ length;
	param->second = angle;
	/*// get normal for p
	Vec3f pln = s.cross(m_axisDir);
	Vec3f plx = m_axisDir.cross(pln);
	Vec3f n;
	if(plx.normalize() < 1.0e-6)
	{
		*param = std::make_pair(0.0f, angle);
		return height;
	}
	if(height < 0)
		n = m_normal[0] * plx - m_normalY;
	else
	n = m_normal[0] * plx + m_normalY;
	Vec3f l = n.cross(pln);
	l.normalize();
	// make sure l points in direction of axis
	if(m_axisDir.dot(l) < 0)
		l *= -1;
	// project p on line m_center + lambda * l
	// get lambda
	float lambda = s.dot(l);
	// make sure l points in direction of axis
	if(m_axisDir.dot(l) < 0)
	{
		if(lambda > 0)
		{
			*param = std::make_pair(s.length(), angle);
			return height;
		}
	}
	else if(lambda < 0)
	{
		*param = std::make_pair(s.length(), angle);
		return height;
	}
	*param = std::make_pair(*fabs(lambda), angle);*/
}

bool Cone::Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
	const Vec3f &n1, const Vec3f &n2, const Vec3f &n3)
{
	//float ncheck = std::max(n2.dot(n3), std::max(n1.dot(n2), n1.dot(n3)));
	//if(ncheck > 0.999)
	//	return false;
	// compute center by intersecting the three planes given by (p1, n1)
	// (p2, n2) and (p3, n3)
	// set up linear system
	double a[4 * 3];
	double d1 = p1.dot(n1);
	double d2 = p2.dot(n2);
	double d3 = p3.dot(n3);
	// column major
	a[0 + 0 * 3] = n1[0];
	a[1 + 0 * 3] = n2[0];
	a[2 + 0 * 3] = n3[0];
	a[0 + 1 * 3] = n1[1];
	a[1 + 1 * 3] = n2[1];
	a[2 + 1 * 3] = n3[1];
	a[0 + 2 * 3] = n1[2];
	a[1 + 2 * 3] = n2[2];
	a[2 + 2 * 3] = n3[2];
	a[0 + 3 * 3] = d1;
	a[1 + 3 * 3] = d2;
	a[2 + 3 * 3] = d3;
	if(dmat_solve(3, 1, a))
		return false;
	m_center[0] = a[0 + 3 * 3];
	m_center[1] = a[1 + 3 * 3];
	m_center[2] = a[2 + 3 * 3];

	// compute axisDir
	Vec3f s1 = p1 - m_center;
	Vec3f s2 = p2 - m_center;
	Vec3f s3 = p3 - m_center;
	s1.normalize();
	s2.normalize();
	s3.normalize();
	Plane pl(s1 + m_center, s2 + m_center, s3 + m_center);
	m_axisDir = pl.getNormal();
	// make sure axis points in direction of s1
	// this defines the side of the cone!!!
	if(m_axisDir.dot(s1) < 0)
		m_axisDir *= -1;
	m_angle = 0;
	float angle = m_axisDir.dot(n1);
	if(angle < -1) // clamp angle to [-1, 1]
		angle = -1;
	else if(angle > 1)
		angle = 1;
	if(angle < 0)
		// m_angle = omega + 90
		angle = std::acos(angle) - float(M_PI) / 2;
	else
		// m_angle = 90 - omega
		angle = float(M_PI) / 2 - std::acos(angle);
	m_angle += angle;
	angle = m_axisDir.dot(n2);
	if(angle < -1) // clamp angle to [-1, 1]
		angle = -1;
	else if(angle > 1)
		angle = 1;
	if(angle < 0)
		// m_angle = omega + 90
		angle = std::acos(angle) - float(M_PI) / 2;
	else
		// m_angle = 90 - omega
		angle = float(M_PI) / 2 - std::acos(angle);
	m_angle += angle;
	angle = m_axisDir.dot(n3);
	if(angle < -1) // clamp angle to [-1, 1]
		angle = -1;
	else if(angle > 1)
		angle = 1;
	if(angle < 0)
		// m_angle = omega + 90
		angle = std::acos(angle) - float(M_PI) / 2;
	else
		// m_angle = 90 - omega
		angle = float(M_PI) / 2 - std::acos(angle);
	m_angle += angle;
	m_angle /= 3;
	if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
		return false;
	//if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
	if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
		return false;
	m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
	m_normalY = m_normal[1] * m_axisDir;
	m_n2d[0] = std::cos(m_angle);
	m_n2d[1] = -std::sin(m_angle);
	m_hcs.FromNormal(m_axisDir);
	m_angularRotatedRadians = 0;
	return true;
}

void DVRClipGeometry::linkContour(      DVRTriangle *startTriangle,
                                        Real32       dist2RefPlane,
                                  const Vec3f       &viewDir, 
                                        bool         positiveWinding)
{
    FDEBUG(("DVRClipGeometry - linkcontour dist = %f\n", dist2RefPlane));
  
    bool closed = false;

    // first, we have to check for the correct winding direction.

    Pnt3f vertex[2];
    bool firstEdge;
    int  first = 0, second = 0;

    if(startTriangle->edgeCut[0] && startTriangle->edgeCut[1])
    {
        vertex[0] = interpolate(startTriangle, 1, 0, dist2RefPlane); 
        vertex[1] = interpolate(startTriangle, 1, 2, dist2RefPlane);

        first = 0; second = 1;
    }
    else if (startTriangle->edgeCut[1] && startTriangle->edgeCut[2])
    {
        vertex[0] = interpolate(startTriangle, 2, 1, dist2RefPlane);
        vertex[1] = interpolate(startTriangle, 2, 0, dist2RefPlane);

        first = 1; second = 2;
    }
    else if (startTriangle->edgeCut[0] && startTriangle->edgeCut[2])
    {
        vertex[0] = interpolate(startTriangle, 0, 1, dist2RefPlane);
        vertex[1] = interpolate(startTriangle, 0, 2, dist2RefPlane);

        first = 0; second = 2;
    }

    // Now we should have both cut points on our edges.

    // If the cross product of the normal of this triangle with the 
    // vector between the two cut points (cutPoint[1] - cutPoint[0]) 
    // has a positive dot product with the viewing direction, then 
    // the edge with cutPoint[0] on it is the right direction, otherwise
    // we would have to choose the other direction.

    Vec3f tmp = vertex[1] - vertex[0];

    tmp = tmp.cross(startTriangle->transformedNormal);

    if(tmp.dot(viewDir) <= 0.0)
    {
        firstEdge = false;
    }else
    {
        firstEdge = true;
    }

    if(!positiveWinding)
        firstEdge = !firstEdge;

    DVRTriangle *current = startTriangle;

    current->inContour = true;

    if(firstEdge)
    {
        current->cutPnt      = vertex[0];
        current->cutPoint[0] = vertex[0][0];
        current->cutPoint[1] = vertex[0][1];
        current->cutPoint[2] = vertex[0][2];

        current->contourNeighbour = &_mfTriangles[current->neighbours[first]];

        //      // debugging -> remove
        //      if(!current->contourNeighbour){
        //        std::cerr<<"contour neighbour is NULL\n";
        //        exit(0);
        //      }

        current = current->contourNeighbour;
    }
    else
    {
        current->cutPnt      = vertex[1];
        current->cutPoint[0] = vertex[1][0];
        current->cutPoint[1] = vertex[1][1];
        current->cutPoint[2] = vertex[1][2];    

        current->contourNeighbour = &_mfTriangles[current->neighbours[second]];
        //      // debugging -> remove
        //      if(!current->contourNeighbour){
        //        std::cerr<<"contour neighbour is NULL\n";
        //        exit(0);
        //      }

        current = current->contourNeighbour;    
    }

    //check neighbours
    while(!closed)
//.........这里部分代码省略.........

    void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
                                     Vec3f& VEC, Vec3f& X, Vec3f& Y)
    {
      Vec3f T;
      BVH_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
      Vec3f TMP;

      T = Q - P;
      A_dot_A = A.dot(A);
      B_dot_B = B.dot(B);
      A_dot_B = A.dot(B);
      A_dot_T = A.dot(T);
      B_dot_T = B.dot(T);

      // t parameterizes ray P,A
      // u parameterizes ray Q,B

      BVH_REAL t, u;

      // compute t for the closest point on ray P,A to
      // ray Q,B

      BVH_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;

      t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;

      // clamp result so t is on the segment P,A

      if((t < 0) || isnan(t)) t = 0; else if(t > 1) t = 1;

      // find u for point on ray Q,B closest to point at t

      u = (t*A_dot_B - B_dot_T) / B_dot_B;

      // if u is on segment Q,B, t and u correspond to
      // closest points, otherwise, clamp u, recompute and
      // clamp t

      if((u <= 0) || isnan(u))
      {
        Y = Q;

        t = A_dot_T / A_dot_A;

        if((t <= 0) || isnan(t))
        {
          X = P;
          VEC = Q - P;
        }
        else if(t >= 1)
        {
          X = P + A;
          VEC = Q - X;
        }
        else
        {
          X = P + A * t;
          TMP = T.cross(A);
          VEC = A.cross(TMP);
        }
      }
      else if (u >= 1)
      {
        Y = Q + B;

        t = (A_dot_B + A_dot_T) / A_dot_A;

        if((t <= 0) || isnan(t))
        {
          X = P;
          VEC = Y - P;
        }
        else if(t >= 1)
        {
          X = P + A;
          VEC = Y - X;
        }
        else
        {
          X = P + A * t;
          T = Y - P;
          TMP = T.cross(A);
          VEC= A.cross(TMP);
        }
      }
      else
      {
        Y = Q + B * u;

        if((t <= 0) || isnan(t))
        {
          X = P;
          TMP = T.cross(B);
          VEC = B.cross(TMP);
        }
        else if(t >= 1)
        {
          X = P + A;
          T = Q - X;
          TMP = T.cross(B);
//.........这里部分代码省略.........