//-----------------------------------------------------------------------
	void Extruder::_extrudeBodyImpl(TriangleBuffer& buffer, const Shape* shapeToExtrude) const
	{
		assert(mExtrusionPath && shapeToExtrude && "Shape and Path must not be null!");
		unsigned int numSegPath = mExtrusionPath->getSegCount();
		unsigned int numSegShape = shapeToExtrude->getSegCount();
		assert(numSegPath>0 && numSegShape>0 && "Shape and path must contain at least two points");
				
		Real totalPathLength = mExtrusionPath->getTotalLength();
		Real totalShapeLength = shapeToExtrude->getTotalLength();
		
		// Merge shape and path with tracks
		Ogre::Real lineicPos=0.;
		Path path = *mExtrusionPath;
		if (mRotationTrack)
			path = path.mergeKeysWithTrack(*mRotationTrack);
		if (mScaleTrack)
			path = path.mergeKeysWithTrack(*mScaleTrack);
		if (mPathTextureTrack)
			path = path.mergeKeysWithTrack(*mPathTextureTrack);
		numSegPath = path.getSegCount();
		Shape shape = *shapeToExtrude;
		if (mShapeTextureTrack)
			shape = shape.mergeKeysWithTrack(*mShapeTextureTrack);
		numSegShape = shape.getSegCount();
		
		// Estimate vertex and index count
		buffer.rebaseOffset();
		buffer.estimateIndexCount(numSegShape*numSegPath*6);
		buffer.estimateVertexCount((numSegShape+1)*(numSegPath+1));
				
		Vector3 oldup;
		for (unsigned int i = 0; i <= numSegPath; ++i)
		{
			Vector3 v0 = path.getPoint(i);
			Vector3 direction = path.getAvgDirection(i);

			Quaternion q = Utils::_computeQuaternion(direction);
						
			Radian angle = (q*Vector3::UNIT_Y).angleBetween(oldup);
			if (i>0 && angle>(Radian)Math::HALF_PI/2.)
			{
				q = Utils::_computeQuaternion(direction, oldup);
			}
			oldup = q * Vector3::UNIT_Y;

			Real scale=1.;
					
			if (i>0) lineicPos += (v0-path.getPoint(i-1)).length();
			
			// Get the values of angle and scale
			if (mRotationTrack)
			{
				Real angle;
				angle = mRotationTrack->getValue(lineicPos, lineicPos / totalPathLength, i);

				q = q*Quaternion((Radian)angle, Vector3::UNIT_Z);
			}
			if (mScaleTrack)
			{
				scale = mScaleTrack->getValue(lineicPos, lineicPos / totalPathLength, i);
			}
			Real uTexCoord;
			if (mPathTextureTrack)
				uTexCoord = mPathTextureTrack->getValue(lineicPos, lineicPos / totalPathLength, i);
			else
				uTexCoord = lineicPos / totalPathLength;
			
			Real lineicShapePos = 0.;
			// Insert new points
			for (unsigned int j =0; j <= numSegShape; ++j)
			{				
				Vector2 vp2 = shapeToExtrude->getPoint(j);
				//Vector2 vp2direction = shapeToExtrude->getAvgDirection(j);
				Vector2 vp2normal = shapeToExtrude->getAvgNormal(j);
				Vector3 vp(vp2.x, vp2.y, 0);
				Vector3 normal(vp2normal.x, vp2normal.y, 0);							
				buffer.rebaseOffset();
				Vector3 newPoint = v0+q*(scale*vp);				
				if (j>0)
					lineicShapePos += (vp2 - shape.getPoint(j-1)).length();
				Real vTexCoord;
				if (mShapeTextureTrack)
					vTexCoord = mShapeTextureTrack->getValue(lineicShapePos, lineicShapePos / totalShapeLength, j);
				else
					vTexCoord = lineicShapePos / totalShapeLength;				

				addPoint(buffer, newPoint,
					q*normal, 
					Vector2(uTexCoord, vTexCoord));

				if (j <numSegShape && i <numSegPath)
				{		
					if (shapeToExtrude->getOutSide() == SIDE_LEFT)
					{
						buffer.triangle(numSegShape + 1, numSegShape + 2, 0);
						buffer.triangle(0, numSegShape + 2, 1);
					}
					else 
					{
						buffer.triangle(numSegShape + 2, numSegShape + 1, 0);
						buffer.triangle(numSegShape + 2, 0, 1);
					}
				}			
			}			
		}			
	}
bool Cylinder::intersectBody( const ray& r, isect& i ) const
{
	double x0 = r.getPosition()[0];
	double y0 = r.getPosition()[1];
	double x1 = r.getDirection()[0];
	double y1 = r.getDirection()[1];

	double a = x1*x1+y1*y1;
	double b = 2.0*(x0*x1 + y0*y1);
	double c = x0*x0 + y0*y0 - 1.0;

	if( 0.0 == a ) {
		// This implies that x1 = 0.0 and y1 = 0.0, which further
		// implies that the ray is aligned with the body of the cylinder,
		// so no intersection.
		return false;
	}

	double discriminant = b*b - 4.0*a*c;

	if( discriminant < 0.0 ) {
		return false;
	}
	
	discriminant = sqrt( discriminant );

	double t2 = (-b + discriminant) / (2.0 * a);

	if( t2 <= RAY_EPSILON ) {
		return false;
	}

	double t1 = (-b - discriminant) / (2.0 * a);

	if( t1 > RAY_EPSILON ) {
		// Two intersections.
		vec3f P = r.at( t1 );
		double z = P[2];
		if( z >= 0.0 && z <= 1.0 ) {
			// It's okay.
			i.t = t1;
			i.N = vec3f( P[0], P[1], 0.0 ).normalize();
			return true;
		}
	}

	vec3f P = r.at( t2 );
	double z = P[2];
	if( z >= 0.0 && z <= 1.0 ) {
		i.t = t2;

		vec3f normal( P[0], P[1], 0.0 );
		// In case we are _inside_ the _uncapped_ cone, we need to flip the normal.
		// Essentially, the cone in this case is a double-sided surface
		// and has _2_ normals
		if( !capped && normal.dot( r.getDirection() ) > 0 )
			normal = -normal;

		i.N = normal.normalize();
		return true;
	}

	return false;
}
Example #3
0
	//-----------------------------------------------------------------------------
	void ManualObject::normal(const Vector3& norm)
	{
		normal(norm.x, norm.y, norm.z);
	}
Example #4
0
void DoAnim()
{
    static double time = 0.0; //Total time running.
    static double artTime = 0.0; //Total time with the current art.

    static DWORD lastTime = 0; //Time of last call.
    const double elapsed = double(GetTickCount() - lastTime) / 1000.0;

    if (lastTime)
    {
        lastTime = GetTickCount();
    }
    else
    {
        lastTime = GetTickCount();
        return;
    }

    time += elapsed;
    artTime += elapsed;

    //If we need new art, get it.
    static CKnot::AutoArt threads;
    static Arrays arrays;
    static FloatArray grid;


    if (!threads.get() || artTime > ResetTime)
    {
        CKnot::StrokeList sl = CreateSquareStrokes();
        sl = RemoveStrokes(sl);

        threads = CKnot::CreateThread(sl);
        artTime = 0.0;

        grid.clear();
        grid.reserve(sl.size() * 10);
        for (CKnot::StrokeList::const_iterator it = sl.begin(); it != sl.end(); ++it)
        {
            grid.push_back(it->a.x);
            grid.push_back(it->a.y);
            grid.push_back(it->type == CKnot::Cross ? 1.0 : 0.0);
            grid.push_back(it->type == CKnot::Glance ? 1.0 : 0.0);
            grid.push_back(it->type == CKnot::Bounce ? 1.0 : 0.0);

            grid.push_back(it->b.x);
            grid.push_back(it->b.y);
            grid.push_back(it->type == CKnot::Cross ? 1.0 : 0.0);
            grid.push_back(it->type == CKnot::Glance ? 1.0 : 0.0);
            grid.push_back(it->type == CKnot::Bounce ? 1.0 : 0.0);
        }

        for (size_t i = 0; i < arrays.size(); ++i)
            delete arrays[i];
        arrays.clear();

        const size_t threadCount = threads->GetThreadCount();

        for (size_t i = 0; i < threadCount; ++i)
        {
            const CKnot::Art::Thread* thread = threads->GetThread(i);
            const CKnot::Art::Z* z = threads->GetZ(i);

            const size_t segsPerKnot = 25;
            const size_t kc = thread->GetKnotCount();

            FloatArray* quads = new FloatArray;
            arrays.push_back(quads);


            const size_t target = kc * segsPerKnot;
            const size_t memSize = 12 * (target + 1);
            quads->reserve(memSize);

            const float scr = double(rand()) / RAND_MAX / 2;
            const float ecr = double(rand()) / RAND_MAX / 2 + .5;
            const float scg = double(rand()) / RAND_MAX / 2;
            const float ecg = double(rand()) / RAND_MAX / 2 + .5;
            const float scb = double(rand()) / RAND_MAX / 2;
            const float ecb = double(rand()) / RAND_MAX / 2 + .5;

            for (size_t i = 0; i <= target; ++i)
            {
                const double s = double(i) / double(target);
                const double t = s;

                const CKnot::vec2 cur = thread->Y(t);
                const CKnot::vec2 dcur = thread->Y(t + .00001);
                const CKnot::vec2 diff = dcur - cur;

                CKnot::vec2 normal(diff.y, -diff.x);
                normal = normal * (1.0 / normal.GetLength());
                normal = normal * .01;
                const CKnot::vec2 flip(-normal.x, -normal.y);

                const CKnot::vec2 start = cur + normal;
                const CKnot::vec2 end = cur + flip;

                const bool over = z->Y(t) > 0.0;

                //Coords
                quads->push_back(start.x);
                quads->push_back(start.y);
                quads->push_back(over ? 0.01 : .1);

                //Colors
                quads->push_back(scr);
                quads->push_back(scg);
                quads->push_back(scb);

                quads->push_back(end.x);
                quads->push_back(end.y);
                quads->push_back(over ? 0.01 : .1);

                quads->push_back(ecr);
                quads->push_back(ecg);
                quads->push_back(ecb);
            }

            assert(quads->size() == memSize);
        }
    }


    //Clear the background some nice color.
    glClearColor(   0.125f + std::sin(time / 2.0) / 8.0,
                    0.125f + std::sin(time / 3.0) / 8.0,
                    0.125f + std::sin(time / 5.0) / 8.0,
                    0.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glEnableClientState(GL_VERTEX_ARRAY);
    glEnableClientState(GL_COLOR_ARRAY);

    for (size_t i = 0; i < arrays.size(); ++i)
    {
        FloatArray& quads = *arrays[i];

        glVertexPointer(3, GL_FLOAT, 24, &quads.front());
        glColorPointer(3, GL_FLOAT, 24, &quads.front() + 3);

        const size_t count = quads.size() / 6;
        const size_t progress = size_t(std::min(artTime / DrawTime, 1.0) * count / 2); //From 0 to .5 of vertices.
        assert(progress >= 0);
        assert(progress <= count / 2);

        size_t start = (count / 2) - progress;
        start += start % 2;

        glDrawArrays(GL_QUAD_STRIP, start, progress * 2);
    }

    //Draw graph
    if (DrawGraph)
    {
        glVertexPointer(2, GL_FLOAT, 20, &grid.front());
        glColorPointer(3, GL_FLOAT, 20, &grid.front() + 2);
        glDrawArrays(GL_LINES, 0, grid.size() / 5);
    }

    glDisableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_COLOR_ARRAY);

    glLoadIdentity();
}
Example #5
0
int Colisao::colisaoJogadorDisco(int j)
{
	static bool antcol[2]; // estado anterior da colisao (true/false)
		
	// Seja P o jogador e Q o disco.
	// Seja P1 a posicao anterior e P2 a posicao atual (o mesmo vale para Q)
	Vetor P1(jog[j]->getOldPos()), P2(jog[j]->getPos());
	Vetor Q1(disco->getOldPos()), Q2(disco->getPos());
	// Calcularemos agora as velocidades
	Vetor VP = P2 - P1;
	Vetor VQ = Q2 - Q1;
	// Definindo variaveis para otimizar o calculo
	Vetor A = P1 - Q1;
	Vetor B = VP - VQ;
	float A2 = A.prodEscalar(A);
	float B2 = B.prodEscalar(B);
	float AB = A.prodEscalar(B);
	float AB2 = AB * AB;
	// quadrado da distancia entre os centros
	float d2 = pow(jog[j]->getRaio() + disco->getRaio(), 2);
	float t; // tempo
	
	// se ainda nao esta colidindo
	if ((jog[j]->getPos() - disco->getPos()).norma() > jog[j]->getRaio() + disco->getRaio()) {
		

		//LD fixes
		float zero = 0.0f;

		// Teste para verificar a nao-colisao
		if (B == zero)  // movimento relativo == 0
			{ antcol[j] = false; return 0; }  // nao houve colisao
		
		// Teste para verificar a nao-colisao
		if (A2 - AB2 / B2 > d2)
			{ antcol[j] = false; return 0; }  // nao houve colisao

		// Teste para verificar a nao colisao
		float raiz = AB2 - B2 * (A2 - d2);
		if (raiz < 0)
			{ antcol[j] = false; return 0; }  // nao houve colisao
		
		// tempo da colisao
		t = (-(AB) - sqrt(raiz)) / B2;

		// Teste para verificar a nao-colisao
		if (!(t > 0 && t <= 1))
			{ antcol[j] = false; return 0; }  // nao houve colisao

		// Verifica se ja houve colisao no instante anterior
		if (antcol[j])
			return 0;
			
		// Atualiza posicoes para o instante de colisao
		jog[j]->setPos(P1 + t * VP);
		disco->setPos(Q1 + t * VQ);	
	}
	else {
		// Atualiza a posicao do disco (para ficar _fora_ do jog[j]->	
		Vetor normal = (disco->getPos() - jog[j]->getPos()).versor() * (disco->getRaio() + jog[j]->getRaio());
		disco->setPos(jog[j]->getPos() + normal);
	}

	// normal (do disco para o jogador)
	Vetor normal(jog[j]->getPos() - disco->getPos());
	normal = (disco->getVel().prodEscalar(normal) / normal.prodEscalar(normal)) * normal; // projecao

	// vetor tangente
	Vetor tangente = disco->getVel() - normal;
	
	// velocidade resultante (da disco)
	disco->setVel(tangente - normal);
	
	// afasta um pouco o disco do jogador
	disco->setPos(disco->getPos() + disco->getVel().versor() * 3);

	// Aumenta velocidade do disco (armengue)
	Vetor v = disco->getVel().versor();
	if (v.norma() == 0)
		v = jog[j]->getVel().versor() * 10.0;
	disco->setVel(disco->getVel() + v * 2.0);

	antcol[j] = true;
	return 1;
}
static void ExtrudeFaces (void* userData, int vertexCount, const dFloat* faceVertec, int id)
{
	dFloat OFFSET = 0.1f;
	dFloat face[32][10];

	NewtonMesh* mesh = (NewtonMesh*) userData;

	// calculate the face normal
	dVector normal (0.0f);
	dVector p0 (faceVertec[0 * 3 + 0], faceVertec[0 * 3 + 1], faceVertec[0 * 3 + 2]);
	dVector p1 (faceVertec[1 * 3 + 0], faceVertec[1 * 3 + 1], faceVertec[1 * 3 + 2]);

	dVector e0 (p1 - p0);
	for (int i = 2; i < vertexCount; i ++) {
		dVector p2 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
		dVector e1 (p2 - p0);

		normal += e0 * e1;
		e0 = e1;
	}
	normal = normal.Scale (1.0f / dSqrt (normal % normal));

	dVector displacemnet (normal.Scale (OFFSET));

	// add the face displace by some offset
	for (int i = 0; i < vertexCount; i ++) {
		dVector p1 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
		p1 += displacemnet;

		face[i][0] = p1.m_x; 
		face[i][1] = p1.m_y;  
		face[i][2] = p1.m_z;   

		face[i][3] = normal.m_x; 
		face[i][4] = normal.m_y;  
		face[i][5] = normal.m_z;  

		face[i][6] = 0.0f; 
		face[i][7] = 0.0f;  
		face[i][8] = 0.0f;  
		face[i][9] = 0.0f;  
	}
	
	// add the face
	NewtonMeshAddFace (mesh, vertexCount, &face[0][0], 10 * sizeof (dFloat), id);


	// now add on face walk the perimeter and add a rivet face
	dVector q0 (faceVertec[(vertexCount - 1) * 3 + 0], faceVertec[(vertexCount - 1) * 3 + 1], faceVertec[(vertexCount - 1) * 3 + 2]);
	q0 += displacemnet;
	for (int i = 0; i < vertexCount; i ++) {
		dVector q1 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
		q1 += displacemnet;

		// calculate the river normal
		dVector edge (q1 - q0);
		dVector n (edge * normal);
		n = n.Scale (1.0f / sqrtf (n % n));

		// build a quad to serve a the face between the two parellel faces
		face[0][0] = q0.m_x; 
		face[0][1] = q0.m_y;  
		face[0][2] = q0.m_z;   
		face[0][3] = n.m_x; 
		face[0][4] = n.m_y;  
		face[0][5] = n.m_z;  
		face[0][6] = 0.0f; 
		face[0][7] = 0.0f;  
		face[0][8] = 0.0f;  
		face[0][9] = 0.0f;  

		face[1][0] = q1.m_x; 
		face[1][1] = q1.m_y;  
		face[1][2] = q1.m_z;   
		face[1][3] = n.m_x; 
		face[1][4] = n.m_y;  
		face[1][5] = n.m_z;  
		face[1][6] = 0.0f; 
		face[1][7] = 0.0f;  
		face[1][8] = 0.0f;  
		face[1][9] = 0.0f;  

		face[2][0] = q1.m_x - displacemnet.m_x; 
		face[2][1] = q1.m_y - displacemnet.m_y;  
		face[2][2] = q1.m_z - displacemnet.m_z;   
		face[2][3] = n.m_x; 
		face[2][4] = n.m_y;  
		face[2][5] = n.m_z;  
		face[2][6] = 0.0f; 
		face[2][7] = 0.0f;  
		face[2][8] = 0.0f;  
		face[2][9] = 0.0f;  

		face[3][0] = q0.m_x - displacemnet.m_x; 
		face[3][1] = q0.m_y - displacemnet.m_y;  
		face[3][2] = q0.m_z - displacemnet.m_z;   
		face[3][3] = n.m_x; 
		face[3][4] = n.m_y;  
		face[3][5] = n.m_z;  
		face[3][6] = 0.0f; 
		face[3][7] = 0.0f;  
		face[3][8] = 0.0f;  
		face[3][9] = 0.0f;  

		// save the first point for the next rivet
		q0 = q1;

		// add this face to the mesh
		NewtonMeshAddFace (mesh, 4, &face[0][0], 10 * sizeof (dFloat), id);
	}
}
Example #7
0
const numeric
fsolve(const ex& f_in, const symbol& x, const numeric& x1, const numeric& x2)
{
	if (!x1.is_real() || !x2.is_real()) {
		throw std::runtime_error("fsolve(): interval not bounded by real numbers");
	}
	if (x1==x2) {
		throw std::runtime_error("fsolve(): vanishing interval");
	}
	// xx[0] == left interval limit, xx[1] == right interval limit.
	// fx[0] == f(xx[0]), fx[1] == f(xx[1]).
	// We keep the root bracketed: xx[0]<xx[1] and fx[0]*fx[1]<0.
	numeric xx[2] = { x1<x2 ? x1 : x2,
	                  x1<x2 ? x2 : x1 };
	ex f;
	if (is_a<relational>(f_in)) {
		f = f_in.lhs()-f_in.rhs();
	} else {
		f = f_in;
	}
	const ex fx_[2] = { f.subs(x==xx[0]).evalf(),
	                    f.subs(x==xx[1]).evalf() };
	if (!is_a<numeric>(fx_[0]) || !is_a<numeric>(fx_[1])) {
		throw std::runtime_error("fsolve(): function does not evaluate numerically");
	}
	numeric fx[2] = { ex_to<numeric>(fx_[0]),
	                  ex_to<numeric>(fx_[1]) };
	if (!fx[0].is_real() || !fx[1].is_real()) {
		throw std::runtime_error("fsolve(): function evaluates to complex values at interval boundaries");
	}
	if (fx[0]*fx[1]>=0) {
		throw std::runtime_error("fsolve(): function does not change sign at interval boundaries");
	}

	// The Newton-Raphson method has quadratic convergence!  Simply put, it
	// replaces x with x-f(x)/f'(x) at each step.  -f/f' is the delta:
	const ex ff = normal(-f/f.diff(x));
	int side = 0;  // Start at left interval limit.
	numeric xxprev;
	numeric fxprev;
	do {
		xxprev = xx[side];
		fxprev = fx[side];
		ex dx_ = ff.subs(x == xx[side]).evalf();
		if (!is_a<numeric>(dx_))
			throw std::runtime_error("fsolve(): function derivative does not evaluate numerically");
		xx[side] += ex_to<numeric>(dx_);
		// Now check if Newton-Raphson method shot out of the interval 
		bool bad_shot = (side == 0 && xx[0] < xxprev) || 
				(side == 1 && xx[1] > xxprev) || xx[0] > xx[1];
		if (!bad_shot) {
			// Compute f(x) only if new x is inside the interval.
			// The function might be difficult to compute numerically
			// or even ill defined outside the interval. Also it's
			// a small optimization. 
			ex f_x = f.subs(x == xx[side]).evalf();
			if (!is_a<numeric>(f_x))
				throw std::runtime_error("fsolve(): function does not evaluate numerically");
			fx[side] = ex_to<numeric>(f_x);
		}
		if (bad_shot) {
			// Oops, Newton-Raphson method shot out of the interval.
			// Restore, and try again with the other side instead!
			xx[side] = xxprev;
			fx[side] = fxprev;
			side = !side;
			xxprev = xx[side];
			fxprev = fx[side];

			ex dx_ = ff.subs(x == xx[side]).evalf();
			if (!is_a<numeric>(dx_))
				throw std::runtime_error("fsolve(): function derivative does not evaluate numerically [2]");
			xx[side] += ex_to<numeric>(dx_);

			ex f_x = f.subs(x==xx[side]).evalf();
			if (!is_a<numeric>(f_x))
				throw std::runtime_error("fsolve(): function does not evaluate numerically [2]");
			fx[side] = ex_to<numeric>(f_x);
		}
		if ((fx[side]<0 && fx[!side]<0) || (fx[side]>0 && fx[!side]>0)) {
			// Oops, the root isn't bracketed any more.
			// Restore, and perform a bisection!
			xx[side] = xxprev;
			fx[side] = fxprev;

			// Ah, the bisection! Bisections converge linearly. Unfortunately,
			// they occur pretty often when Newton-Raphson arrives at an x too
			// close to the result on one side of the interval and
			// f(x-f(x)/f'(x)) turns out to have the same sign as f(x) due to
			// precision errors! Recall that this function does not have a
			// precision goal as one of its arguments but instead relies on
			// x converging to a fixed point. We speed up the (safe but slow)
			// bisection method by mixing in a dash of the (unsafer but faster)
			// secant method: Instead of splitting the interval at the
			// arithmetic mean (bisection), we split it nearer to the root as
			// determined by the secant between the values xx[0] and xx[1].
			// Don't set the secant_weight to one because that could disturb
			// the convergence in some corner cases!
			constexpr double secant_weight = 0.984375;  // == 63/64 < 1
			numeric xxmid = (1-secant_weight)*0.5*(xx[0]+xx[1])
			    + secant_weight*(xx[0]+fx[0]*(xx[0]-xx[1])/(fx[1]-fx[0]));
			ex fxmid_ = f.subs(x == xxmid).evalf();
			if (!is_a<numeric>(fxmid_))
				throw std::runtime_error("fsolve(): function does not evaluate numerically [3]");
			numeric fxmid = ex_to<numeric>(fxmid_);
			if (fxmid.is_zero()) {
				// Luck strikes...
				return xxmid;
			}
			if ((fxmid<0 && fx[side]>0) || (fxmid>0 && fx[side]<0)) {
				side = !side;
			}
			xxprev = xx[side];
			fxprev = fx[side];
			xx[side] = xxmid;
			fx[side] = fxmid;
		}
	} while (xxprev!=xx[side]);
	return xxprev;
}
  void HintedHandleEstimator::estimate(
    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
    const geometry_msgs::PointStampedConstPtr &point_msg)
  {
    boost::mutex::scoped_lock lock(mutex_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    int K = 1;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);

    pcl::fromROSMsg(*cloud_msg, *cloud);
    geometry_msgs::PointStamped transed_point;
    ros::Time now = ros::Time::now();
    try
      {
      listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
      listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
    }
    catch(tf::TransformException ex)
      {
      JSK_ROS_ERROR("%s", ex.what());
      return;
    }
    pcl::PointXYZ searchPoint;
    searchPoint.x = transed_point.point.x;
    searchPoint.y = transed_point.point.y;
    searchPoint.z = transed_point.point.z;

    //remove too far cloud
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
    pass.filter(*cloud);
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
    pass.filter(*cloud);
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
    pass.filter(*cloud);

    if(cloud->points.size() < 10){
      JSK_ROS_INFO("points are too small");
      return;
    }
    if(1){ //estimate_normal
      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
      ne.setInputCloud(cloud);
      ne.setSearchMethod(kd_tree);
      ne.setRadiusSearch(0.02);
      ne.setViewPoint(0, 0, 0);
      ne.compute(*cloud_normals);
    }
    else{ //use normal of msg
      
    }
    if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
      JSK_ROS_INFO("kdtree failed");
      return;
    }
    float x = cloud->points[pointIdxNKNSearch[0]].x;
    float y = cloud->points[pointIdxNKNSearch[0]].y;
    float z = cloud->points[pointIdxNKNSearch[0]].z;
    float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
    float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
    float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
    double theta = acos(v_x);
    // use normal for estimating handle direction
    tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
    tf::Quaternion final_quaternion = normal;
    double min_theta_index = 0;
    double min_width = 100;
    tf::Quaternion min_qua(0, 0, 0, 1);
    visualization_msgs::Marker debug_hand_marker;
    debug_hand_marker.header = cloud_msg->header;
    debug_hand_marker.ns = string("debug_grasp");
    debug_hand_marker.id = 0;
    debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
    debug_hand_marker.pose.orientation.w = 1;
    debug_hand_marker.scale.x=0.003;
    tf::Matrix3x3 best_mat;
    //search 180 degree and calc the shortest direction
    for(double theta_=0; theta_<3.14/2; 
        theta_+=3.14/2/30){
      tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
      tf::Quaternion temp_qua = normal * rotate_;
      tf::Matrix3x3 temp_mat(temp_qua);
      geometry_msgs::Pose pose_respected_to_tf;
      pose_respected_to_tf.position.x = x;
      pose_respected_to_tf.position.y = y;
      pose_respected_to_tf.position.z = z;
      pose_respected_to_tf.orientation.x = temp_qua.getX();
      pose_respected_to_tf.orientation.y = temp_qua.getY();
      pose_respected_to_tf.orientation.z = temp_qua.getZ();
      pose_respected_to_tf.orientation.w = temp_qua.getW();
      Eigen::Affine3d box_pose_respected_to_cloud_eigend;
      tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
      Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
        = box_pose_respected_to_cloud_eigend.inverse();
      Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
      Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
        = box_pose_respected_to_cloud_eigend_inversed.matrix();
      jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
                                                                    box_pose_respected_to_cloud_eigen_inversed_matrixd,
                                                                    box_pose_respected_to_cloud_eigen_inversed_matrixf);
      Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::transformPointCloud(*cloud, *output_cloud, offset);

      pcl::PassThrough<pcl::PointXYZ> pass;
      pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
      pass.setInputCloud(output_cloud);
      pass.setFilterFieldName("y");
      pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
      pass.filter(*points_z);
      pass.setInputCloud(points_z);
      pass.setFilterFieldName("z");
      pass.setFilterLimits(-handle.finger_d, handle.finger_d);
      pass.filter(*points_yz);
      pass.setInputCloud(points_yz);
      pass.setFilterFieldName("x");
      pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
      pass.filter(*points_xyz);
      pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
      for(size_t index=0; index<points_xyz->size(); index++){
        points_xyz->points[index].x = points_xyz->points[index].z = 0;
      }
      if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
      kdtree.setInputCloud(points_xyz);
      std::vector<int> pointIdxRadiusSearch;
      std::vector<float> pointRadiusSquaredDistance;
      pcl::PointXYZ search_point_tree;
      search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
      if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
        double before_w=10, temp_w;
        for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
          temp_w =sqrt(pointRadiusSquaredDistance[index]);
          if(temp_w - before_w > handle.finger_w*2){
            break; // there are small space for finger
          }
          before_w=temp_w;
        }
        if(before_w < min_width){
          min_theta_index = theta_;
          min_width = before_w;
          min_qua = temp_qua;
          best_mat = temp_mat;
        }
        //for debug view
        geometry_msgs::Point temp_point;
        std_msgs::ColorRGBA temp_color;
        temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
        temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
        temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
        temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
        debug_hand_marker.points.push_back(temp_point);
        debug_hand_marker.colors.push_back(temp_color);
        temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
        temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
        temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
        debug_hand_marker.points.push_back(temp_point);
        debug_hand_marker.colors.push_back(temp_color);
      }
    }
    geometry_msgs::PoseStamped handle_pose_stamped;
    handle_pose_stamped.header = cloud_msg->header;
    handle_pose_stamped.pose.position.x = x;
    handle_pose_stamped.pose.position.y = y;
    handle_pose_stamped.pose.position.z = z;
    handle_pose_stamped.pose.orientation.x = min_qua.getX();
    handle_pose_stamped.pose.orientation.y = min_qua.getY();
    handle_pose_stamped.pose.orientation.z = min_qua.getZ();
    handle_pose_stamped.pose.orientation.w = min_qua.getW();
    std_msgs::Float64 min_width_msg;
    min_width_msg.data = min_width;
    pub_pose_.publish(handle_pose_stamped);
    pub_debug_marker_.publish(debug_hand_marker);
    pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
    jsk_recognition_msgs::SimpleHandle simple_handle;
    simple_handle.header = handle_pose_stamped.header;
    simple_handle.pose = handle_pose_stamped.pose;
    simple_handle.handle_width = min_width;
    pub_handle_.publish(simple_handle);
  }
Example #9
0
osg::Vec3dArray*
BoundaryUtil::findMeshBoundary( osg::Node* node, bool geocentric )
{
    // the normal defines the XY plane in which to search for a boundary
    osg::Vec3d normal(0,0,1);

    if ( geocentric )
    {
        // define the XY plane based on the normal to the center of the dataset:
        osg::BoundingSphere bs = node->getBound();
        normal = bs.center();
        normal.normalize();
    }

    osg::ref_ptr<osg::Vec3dArray> _result = new osg::Vec3dArray();

    // first build a topology graph from the node.
    TopologyGraph topology;

    // set up a quat that will rotate geometry into our XY plane
    if ( normal != osg::Vec3(0,0,1) )
        topology._rot.makeRotate( normal, osg::Vec3d(0,0,1) );

    // build the topology
    BuildTopologyVisitor buildTopoVisitor(topology);
    node->accept( buildTopoVisitor );

    // starting with the minimum-Y vertex (which is guaranteed to be in the boundary)
    // traverse the outside of the point set. Do this by sorting all the edges by
    // their angle relative to the vector to the previous point. The vector with the
    // smallest angle represents the edge connecting the current point to the next
    // boundary point. Walk the edge until we return to the beginning.
    
    Index vptr      = topology._minY;
    Index vptr_prev = topology._verts.end();

    while( true )
    {
        // store this vertex in the result set:
        _result->push_back( *vptr );

        // pull up the next 2D vertex (XY plane):
        osg::Vec2d vert( vptr->x(), vptr->y() );

        // construct the "base" vector that points from the current point back
        // to the previous point; or to -X in the initial case
        osg::Vec2d base;
        if ( vptr_prev == topology._verts.end() )
            base.set( -1, 0 );
        else
            base = osg::Vec2d( vptr_prev->x(), vptr_prev->y() ) - vert;
            
        // pull up the edge set for this vertex:
        IndexSet& edges = topology._edgeMap[vptr];

        // find the edge with the minimun delta angle to the base vector
        double minAngle = DBL_MAX;
        Index  minEdge  = topology._verts.end();

        for( IndexSet::iterator e = edges.begin(); e != edges.end(); ++e )
        {
            // don't go back from whence we just came
            if ( *e == vptr_prev )
              continue;

            // calculate the angle between the base vector and the current edge:
            osg::Vec2d edgeVert( (*e)->x(), (*e)->y() );
            osg::Vec2d edge = edgeVert - vert;

            double baseAngle = atan2(base.y(), base.x());
            double edgeAngle = atan2(edge.y(), edge.x());

            double outsideAngle = baseAngle - edgeAngle;

            // normalize it to [0..360)
            if ( outsideAngle < 0.0 )
              outsideAngle += 2.0*osg::PI;

            // see it is qualifies as the new minimum angle
            if ( outsideAngle < minAngle )
            {
                minAngle = outsideAngle;
                minEdge = *e;
            }
        }

        if ( minEdge == topology._verts.end() )
        {
            // this will probably never happen
            osg::notify(osg::WARN) << "Illegal state - bailing" << std::endl;
            return 0L;
        }

        vptr_prev = vptr;

        // follow the chosen edge around the outside of the geometry:
        vptr = minEdge;

        // once we make it all the way around, we're done:
        if ( vptr == topology._minY )
            break;
    }

    // un-rotate the results from the XY plane back to their original frame:
    osg::Quat invRot = topology._rot.inverse();
    for( osg::Vec3dArray::iterator i = _result->begin(); i != _result->end(); ++i )
    {
      (*i) = invRot * (*i);
    }

    return _result.release();
}
Example #10
0
void main()
{
    bool patternfound = false;
    bool reset = false;
    bool resetAuto = false;
    int nbImages = 0;
    double moyFinale = 0;
    char key = 0;
    bool detectionMire = false;
	bool detectionVisage = false;
	int cpt = 0, moyCpt = 0, i = 0;

	std::cout << "initialisation de Chehra..." << std::endl;
	Chehra chehra;
	std::cout << "done" << std::endl;

    cv::TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03);
    cv::Size winSize(31, 31);
    
    cv::Mat cameraMatrix, distCoeffs;
    cv::Mat imCalib;
    cv::Mat imCalibColor;
    cv::Mat imCalibNext;
    cv::Mat rvecs, tvecs;
    cv::Mat Rc, C = cv::Mat(3, 1, CV_64F), rotVecInv;
    
    std::vector<cv::Point2f> imagePoints;
    std::vector<cv::Point3f> objectPoints;
    std::vector<cv::Point3f> cubeObjectPoints;
	std::vector<cv::Point3f> dessinPointsVisage;
    std::vector<std::vector<cv::Point2f>> chessCornersInit(2);
	std::vector<std::vector<cv::Point2f>> pointsVisageInit(2);
    std::vector<cv::Point3f> chessCorners3D;
	std::vector<cv::Point3f> pointsVisage3D;
	std::vector<cv::Point3f> visage;
    std::vector<double> distances;
    double moyDistances;

    // Creation des coins de la mire
    for(int x = 0; x < COLCHESSBOARD; x++)
        for(int y = 0; y < ROWCHESSBOARD; y++)
            chessCorners3D.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f));  

    // Creation des points a projeter
    for(int x = 0; x < COLCHESSBOARD; x++)
        for(int y = 0; y < ROWCHESSBOARD; y++)
            objectPoints.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f));
	
	cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);

	fs["cameraMatrix"] >> cameraMatrix;
	fs["distCoeffs"] >> distCoeffs;

	double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np 
	//mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane
	double g = 2000 * f; // je sais pas pourquoi. au pif.

	fs.release();

	cv::VideoCapture vcap(0); 
	if(!vcap.isOpened()){
		std::cout << "FAIL!" << std::endl;
		return;
	}

	cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3));

	do
	{
		vcap >> *frame;
	}while(frame->empty());

	osg::ref_ptr<osg::Image> backgroundImage = new osg::Image;
	backgroundImage->setImage(frame->cols, frame->rows, 3,
		GL_RGB, GL_BGR, GL_UNSIGNED_BYTE,
		(uchar*)(frame->data),
		osg::Image::AllocationMode::NO_DELETE, 1);

	// read the scene from the list of file specified commandline args.
	osg::ref_ptr<osg::Group> group = new osg::Group;
	osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f);

	std::cout << "initialisation de l'objet 3D..." << std::endl;
	osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/Creature.obj");
	std::cout << "done" << std::endl;
   
	osg::StateSet* obectStateset = objet3D->getOrCreateStateSet();
       obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
	osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform();
	osg::ref_ptr<osg::PositionAttitudeTransform> pat = new osg::PositionAttitudeTransform();

	// construct the viewer.
	osgViewer::CompositeViewer compositeViewer;
	osgViewer::View* viewer = new osgViewer::View;
	osgViewer::View* viewer2 = new osgViewer::View;

	// add the HUD subgraph.
	group->addChild(cam);

	mat->addChild(objet3D);
	pat->addChild(mat);
	group->addChild(pat);

    pat->setScale(osg::Vec3d(3, 3, 3));

	osg::Matrixd projectionMatrix;

	projectionMatrix.makeFrustum(
		-cameraMatrix.at<double>(0, 2),		vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2),
		-cameraMatrix.at<double>(1, 2),		vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2),
		f,								g);

	osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f);

	// set the scene to render
	viewer->setSceneData(group.get());
	viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2); 
	viewer->getCamera()->setProjectionMatrix(projectionMatrix);
	viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal);

	viewer2->setSceneData(group.get());
	viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2); 
	viewer2->getCamera()->setProjectionMatrix(projectionMatrix);
	osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f);
	viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2);

	compositeViewer.addView(viewer);
	compositeViewer.addView(viewer2);

	compositeViewer.realize();  // set up windows and associated threads.



    do
    {       
		group->removeChild(pat);
        patternfound = false;
        resetAuto = false;
        detectionMire = false;
		detectionVisage = false;
            
        imagePoints.clear();
        chessCornersInit[0].clear();
        chessCornersInit[1].clear();
		pointsVisageInit[0].clear();
		pointsVisageInit[1].clear();
		pointsVisage3D.clear();
		dessinPointsVisage.clear();
		visage.clear();
        moyDistances = 0;
        distances.clear();
        imCalibNext.release();
        
        std::cout << "recherche de pattern" << std::endl;

		time_t start = clock();
		double timer = 0;
		
        do
        {
			start = clock();

            vcap >> *frame;

			backgroundImage->dirty();
            //detectionMire = detecterMire(frame, &chessCornersInit[1], &imCalibNext);
			detectionVisage = detecterVisage(frame, &chehra, &pointsVisageInit[1], &visage, &pointsVisage3D, &imCalibNext);

			cpt++;
			double duree = (clock() - start)/(double) CLOCKS_PER_SEC;
			timer += duree;

			if(timer >= 1){
				std::cout << cpt << " fps" << std::endl;
				moyCpt += cpt;
				timer = 0;
				duree = 0;
				i++;
				cpt = 0;
				start = clock();
			}

            compositeViewer.frame();
        }while(!detectionMire && !detectionVisage && !compositeViewer.done());

        if(compositeViewer.done())
            break;

        std::cout << "pattern detectee" << std::endl << std::endl;

		group->addChild(pat);
		
        do
        {           
			start = clock();

            vcap >> *frame;
            
			cv::Mat rotVec = trackingMire(frame, &imCalibNext, &pointsVisageInit, &pointsVisage3D, &cameraMatrix, &distCoeffs, &tvecs);
            //cv::Mat rotVec = trackingMire(frame, &imCalibNext, &chessCornersInit, &chessCorners3D, &cameraMatrix, &distCoeffs, &tvecs);

            //imagePoints = dessinerPoints(frame, objectPoints, rotVec, tvecs, cameraMatrix, distCoeffs);
			imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs);
            
            double r11 = rotVec.at<double>(0, 0);
            double r21 = rotVec.at<double>(1, 0);
            double r31 = rotVec.at<double>(2, 0);
            double r32 = rotVec.at<double>(2, 1);
            double r33 = rotVec.at<double>(2, 2);

			osg::Matrixd matrixR;
            matrixR.makeRotate(
                atan2(r32, r33), osg::Vec3d(1.0, 0.0, 0.0),
                -atan2(-r31, sqrt((r32 * r32) + (r33 * r33))), osg::Vec3d(0.0, 0.0, 1.0),
                atan2(r21, r11), osg::Vec3d(0.0, 1.0, 0.0));
            
            mat->setMatrix(matrixR);
			pat->setPosition(osg::Vec3d(tvecs.at<double>(0, 0), tvecs.at<double>(2, 0), -tvecs.at<double>(1, 0)));

			//std::cout << "x = " << tvecs.at<double>(0, 0) << " - y = " << tvecs.at<double>(1, 0) << " - z = " << tvecs.at<double>(2, 0) << std::endl;

            // Calcul d'erreur de reprojection
            double moy = 0;
            for(int j = 0; j < pointsVisageInit[1].size() ; j++)
			{
				double d = sqrt(pow(pointsVisageInit[0][j].y - imagePoints[j].y, 2) + pow(pointsVisageInit[0][j].x - imagePoints[j].x, 2));
				distances.push_back(d);
				moy += d;
			}

            moyDistances = moy / pointsVisageInit[1].size();

            if(moyDistances > 1) // si l'ecart de reproj est trop grand, reset
                resetAuto = true;

			double duree = (clock() - start)/(double) CLOCKS_PER_SEC;


				std::cout << (int)(1/duree) << " fps" << std::endl;
				moyCpt += (int)(1/duree);
				duree = 0;
				i++;
			
            backgroundImage->dirty();
            compositeViewer.frame();
        }while(!compositeViewer.done() && !resetAuto);
		
    }while(!compositeViewer.done());

	std::cout << std::endl << "Moyenne des fps : " << moyCpt/i << std::endl;

	std::system("PAUSE");
}
Example #11
0
// Extent the velocities to "air"
void FluidSolver::VelocityExtension()
{
  int iterations = 0;
  float narrowbandWidth = 0;
  float dt = 0.7;
  std::set<LevelSet *>::const_iterator iter = mFluids.begin();
  std::set<LevelSet *>::const_iterator iend = mFluids.end();
  while (iter != iend) {
    iterations = std::max(iterations, (int)((*iter)->GetNarrowBandWidth()*0.5f/dt));
    narrowbandWidth = std::max(narrowbandWidth, (*iter)->GetNarrowBandWidth()*(*iter)->GetDx()*0.5f);
    iter++;
  }

  iterations = std::min(iterations, (int)(mVoxels.GetDimX()/dt));
  iterations = std::min(iterations, (int)(mVoxels.GetDimY()/dt));
  iterations = std::min(iterations, (int)(mVoxels.GetDimZ()/dt));

  std::cerr << "Velocity extension (" << iterations << " iterations)..." << std::endl;
  for (int iter = 0; iter < iterations; iter++) {

    Volume<Vector3<float> > velocities = mVelocityField;

    for (int i = 0; i < mVoxels.GetDimX(); i++) {
      for (int j = 0; j < mVoxels.GetDimY(); j++) {
        for (int k = 0; k < mVoxels.GetDimZ(); k++) {

          const float val = mVoxels.GetValue(i,j,k);

          // Extend only in air or solid (don't extend beyond narrowband for efficiency)
          if (IsFluid(i,j,k) || val >= narrowbandWidth) continue;

          Vector3<float> normal(mVoxels.GetValue(i+1,j,k) - mVoxels.GetValue(i-1,j,k),
                                mVoxels.GetValue(i,j+1,k) - mVoxels.GetValue(i,j-1,k),
                                mVoxels.GetValue(i,j,k+1) - mVoxels.GetValue(i,j,k-1));

          if (normal.Length() > 0)
            normal.Normalize();

          Vector3<float> v = mVelocityField.GetValue(i,j,k);

          Vector3<float> ip = mVelocityField.GetValue(i+1,j,k);
          Vector3<float> im = mVelocityField.GetValue(i-1,j,k);

          Vector3<float> jp = mVelocityField.GetValue(i,j+1,k);
          Vector3<float> jm = mVelocityField.GetValue(i,j-1,k);

          Vector3<float> kp = mVelocityField.GetValue(i,j,k+1);
          Vector3<float> km = mVelocityField.GetValue(i,j,k-1);

          Vector3<float> v0 =  v - dt*(std::max(0.0f, normal[0])*(v - im) + std::min(0.0f, normal[0])*(ip - v) +
                                       std::max(0.0f, normal[1])*(v - jm) + std::min(0.0f, normal[1])*(jp - v) +
                                       std::max(0.0f, normal[2])*(v - km) + std::min(0.0f, normal[2])*(kp - v));

          velocities.SetValue(i,j,k, v0);
        }
      }
    }

    mVelocityField = velocities;
  }
}
Example #12
0
// play CA2 event (us vs them game for ng no. of groups)
void event_CA2(int ng)
{
  int i, j, k, *g, *sg, preexists;
  double wgx, xb;    // sum of groups contribution with Beta exponent
  
  g = malloc(ng*sizeof(int));       // array for group index of selected group in its polity  
  sg = malloc(ng*sizeof(int));      // array for selected unique group indices
  
  // choose ng no. of groups randomly
  for(k = 0; k < ng; k++){
    do{
      g[k] = rnd(G);            // selection from uniform distribution; // generate random no. less than total no. of groups
      preexists = 0;
      for(j = 0; j < k; j++){      // check if it already exists in the selected group array 'sg' for playing game
	  if(g[k] == sg[j]){
	    preexists = 1; break;
	  }
	}
    }while(preexists == 1);

    sg[k] = g[k];
  }
   
  // play us vs them game
  B = normal(Bo, Sigma_B);                // randomly choose Benefit B
  if(B < 0) B = 0;                     // check if B is less than zero
  update_XP_CA2(g, ng);        // caculate probability of success of ng groups in array pbs    
  
  for(k = 0; k < ng; k++){
    updateIndividualPayoffAfterProduction(g[k], ng, P[g[k]]);          // update payoff of individuals in each group and of each group 
#if PUNISH
    punish(g[k]);
#endif
    updateGroupPayoff(g[k]);
  } 
    
  for(wgx = 0, k = 0; k < ng; k++)
    wgx += pow(X[g[k]], Beta);        // sum of group contribution with Beta exponent
  
  for(k = 0; k < ng; k++){    
    Tx    = malloc( GS[g[k]]*sizeof(double [TRAITS]));
    Xmax  = malloc(GS[g[k]]*sizeof(double));
    // get state of strategies before update
    for(i = 0; i < GS[g[k]]; i++){
      Tx[i][0] = x[g[k]][i];
      Tx[i][1] = dxi[g[k]][i];
      Tx[i][2] = dsi[g[k]][i];      
#if Accumulatedpayoff      
  Xmax[i] = pow( ( (1-Discount)*Api[g[k]][i] + (1.0 + B*V[g[k]][i]) )/C, I_Alpha );         // upper bound of x at each role 
#else
  Xmax[i] = pow( (1.0 + B*V[g[k]][i])/C, I_Alpha);                                 
#endif 
    }    
#if PUNISH
    Lookup = malloc(2*(GS[g[k]]-1)*sizeof(int[3]));                                     // allocate memory for lookup table  
    Lidx   = malloc(2*(GS[g[k]]-1)*sizeof(int));
    #if FORESIGHT
      Strat = malloc(GS[g[k]]*sizeof(double[TRAITS]));      
      FS = malloc(GS[g[k]]*sizeof(double[TRAITS]));
      Lt = malloc(2*(GS[g[k]]-1)*sizeof(int[3])); 
      Idx = malloc(2*(GS[g[k]]-1)*sizeof(int));
    #endif
#endif  
    // update strategies of individual with probability Mu
    for(i = 0; i < GS[g[k]]; i++){   // through every individual in group
      if( U01() < Mu){          // if random number is less than Mu, then update strategy
	xb = wgx - pow( X[g[k]], Beta );
	xb = MAX(xb, 0.001);
	updateStrategy(g[k], i, ng, xb);    // use quantal response approach to update strategy (each individual strategy is updated believing other individuals do not change)
      }
    }
    free(Tx); 
#if PUNISH
    free(Lookup); free(Lidx);  
    #if FORESIGHT
      free(Strat); free(FS); free(Lt); free(Idx);    
    #endif
#endif 
  }
  free(g); free(sg); free(Xmax); 
}
Example #13
0
float Random::getRealNormal(float p_min, float p_max, Generator p_generator/*= Generator::DETERMINISTIC*/)
{
	std::normal_distribution<float> normal(p_min, p_max);
	float res = normal(*getEnginebyType(p_generator));
	return res;
}
Example #14
0
      void Smooth::operator() (const Mesh& in, Mesh& out) const
      {
        std::unique_ptr<ProgressBar> progress;
        if (message.size())
          progress.reset (new ProgressBar (message, 8));
        out.clear();

        const size_t V = in.num_vertices();
        if (!V) return;

        if (in.num_quads())
          throw Exception ("For now, mesh smoothing is only supported for triangular meshes");
        const size_t T = in.num_triangles();
        if (V == 3*T)
          throw Exception ("Cannot perform smoothing on this mesh: no triangulation information");

        // Pre-compute polygon centroids and areas
        VertexList centroids;
        vector<default_type> areas;
        for (TriangleList::const_iterator p = in.triangles.begin(); p != in.triangles.end(); ++p) {
          centroids.push_back ((in.vertices[(*p)[0]] + in.vertices[(*p)[1]] + in.vertices[(*p)[2]]) * (1.0/3.0));
          areas.push_back (area (in, *p));
        }
        if (progress) ++(*progress);

        // Perform pre-calculation of an appropriate mesh neighbourhood for each vertex
        // Use knowledge of the connections between vertices provided by the triangles/quads to
        //   perform an iterative search outward from each vertex, selecting a subset of
        //   polygons for each vertex
        // Extent of window should be approximately the value of spatial_factor, though only an
        //   approximate windowing is likely to be used (i.e. number of iterations)
        //
        // Initialisation is different to iterations: Need a single pass to find those
        //   polygons that actually use the vertex
        vector< std::set<uint32_t> > vert_polys (V, std::set<uint32_t>());
        // For each vertex, don't just want to store the polygons within the neighbourhood;
        //   also want to store those that will be expanded from in the next iteration
        vector< vector<uint32_t> > vert_polys_to_expand (V, vector<uint32_t>());

        for (uint32_t t = 0; t != T; ++t) {
          for (uint32_t i = 0; i != 3; ++i) {
            vert_polys[(in.triangles[t])[i]].insert (t);
            vert_polys_to_expand[(in.triangles[t])[i]].push_back (t);
          }
        }
        if (progress) ++(*progress);

        // Now, we want to expand this selection outwards for each vertex
        // To do this, also want to produce a list for each polygon: containing those polygons
        //   that share a common edge (i.e. two vertices)
        vector< vector<uint32_t> > poly_neighbours (T, vector<uint32_t>());
        for (uint32_t i = 0; i != T; ++i) {
          for (uint32_t j = i+1; j != T; ++j) {
            if (in.triangles[i].shares_edge (in.triangles[j])) {
              poly_neighbours[i].push_back (j);
              poly_neighbours[j].push_back (i);

            }
          }
        }
        if (progress) ++(*progress);

        // TODO Will want to develop a better heuristic for this
        for (size_t iter = 0; iter != 8; ++iter) {
          for (uint32_t v = 0; v != V; ++v) {

            // Find polygons at the outer edge of this expanding front, and add them to the neighbourhood for this vertex
            vector<uint32_t> next_front;
            for (vector<uint32_t>::const_iterator front = vert_polys_to_expand[v].begin(); front != vert_polys_to_expand[v].end(); ++front) {
              for (vector<uint32_t>::const_iterator expansion = poly_neighbours[*front].begin(); expansion != poly_neighbours[*front].end(); ++expansion) {
                const std::set<uint32_t>::const_iterator existing = vert_polys[v].find (*expansion);
                if (existing == vert_polys[v].end()) {
                  vert_polys[v].insert (*expansion);
                  next_front.push_back (*expansion);
                }
              }
            }
            vert_polys_to_expand[v] = std::move (next_front);

          }
        }
        if (progress) ++(*progress);



        // Need to perform a first mollification pass, where the polygon normals are
        //   smoothed but the vertices are not perturbed
        // However, in order to calculate these new normals, we need to calculate new vertex positions!
        VertexList mollified_vertices;
        // Use half standard spatial factor for mollification
        // Denominator = 2(SF/2)^2
        const default_type spatial_mollification_power_multiplier = -2.0 / Math::pow2 (spatial);
        // No need to normalise the Gaussian; have to explicitly normalise afterwards
        for (uint32_t v = 0; v != V; ++v) {

          Vertex new_pos (0.0, 0.0, 0.0);
          default_type sum_weights = 0.0;

          // For now, just use every polygon as part of the estimate
          // Eventually, restrict this to some form of mesh neighbourhood
          //for (size_t i = 0; i != centroids.size(); ++i) {
          for (std::set<uint32_t>::const_iterator it = vert_polys[v].begin(); it != vert_polys[v].end(); ++it) {
            const uint32_t i = *it;
            default_type this_weight = areas[i];
            const default_type distance_sq = (centroids[i] - in.vertices[v]).squaredNorm();
            this_weight *= std::exp (distance_sq * spatial_mollification_power_multiplier);
            const Vertex prediction = centroids[i];
            new_pos += this_weight * prediction;
            sum_weights += this_weight;
          }

          new_pos *= (1.0 / sum_weights);
          mollified_vertices.push_back (new_pos);

        }
        if (progress) ++(*progress);

        // Have new vertices; compute polygon normals based on these vertices
        Mesh mollified_mesh;
        mollified_mesh.load (mollified_vertices, in.triangles);
        VertexList tangents;
        for (TriangleList::const_iterator p = mollified_mesh.triangles.begin(); p != mollified_mesh.triangles.end(); ++p)
          tangents.push_back (normal (mollified_mesh, *p));
        if (progress) ++(*progress);

        // Now perform the actual smoothing
        const default_type spatial_power_multiplier = -0.5 / Math::pow2 (spatial);
        const default_type influence_power_multiplier = -0.5 / Math::pow2 (influence);
        for (size_t v = 0; v != V; ++v) {

          Vertex new_pos (0.0, 0.0, 0.0);
          default_type sum_weights = 0.0;

          //for (size_t i = 0; i != centroids.size(); ++i) {
          for (std::set<uint32_t>::const_iterator it = vert_polys[v].begin(); it != vert_polys[v].end(); ++it) {
            const uint32_t i = *it;
            default_type this_weight = areas[i];
            const default_type distance_sq = (centroids[i] - in.vertices[v]).squaredNorm();
            this_weight *= std::exp (distance_sq * spatial_power_multiplier);
            const default_type prediction_distance = (centroids[i] - in.vertices[v]).dot (tangents[i]);
            const Vertex prediction = in.vertices[v] + (tangents[i] * prediction_distance);
            this_weight *= std::exp (Math::pow2 (prediction_distance) * influence_power_multiplier);
            new_pos += this_weight * prediction;
            sum_weights += this_weight;
          }

          new_pos *= (1.0 / sum_weights);
          out.vertices.push_back (new_pos);

        }
        if (progress) ++(*progress);

        out.triangles = in.triangles;

        // If the vertex normals were calculated previously, re-calculate them
        if (in.have_normals())
          out.calculate_normals();

      }
Example #15
0
VECT surface_height::height_normal_get(int x, int y, MASK_INTERFACE *p_mask,
                                       int mask_x, int mask_y)
{
  VECT2DI list[] = 
  { 
    VECT2DI( 0, 1),
    VECT2DI( 1, 1),
    VECT2DI( 1, 0),
    VECT2DI( 1,-1),
    VECT2DI( 0,-1),
    VECT2DI(-1,-1),
    VECT2DI(-1, 0),
    VECT2DI(-1, 1),
    VECT2DI( 0, 1)
  };
  VECT vects[] = 
  {
    VECT3DF( 0, 0, 1),
    VECT3DF( 1, 0, 1),
    VECT3DF( 1, 0, 0),
    VECT3DF( 1, 0,-1),
    VECT3DF( 0, 0,-1),
    VECT3DF(-1, 0,-1),
    VECT3DF(-1, 0, 0),
    VECT3DF(-1, 0, 1),
    VECT3DF( 0, 0, 1)
  };
  
  int   points = (int)(sizeof(list)/sizeof(list[0]));
  
  float height_current = height_get(x, y);
  
  int i;
  for(i = 0; i < points; i++) {
    tpos ax = x+list[i].x;
    tpos ay = y+list[i].y;
    if(pixel_valid(ax, ay) && (!p_mask || p_mask->mask_get(mask_x+ax,mask_y+ay))) {
      float height = height_get(ax, ay);
      vects[i].y = (height - height_current)*pixel_height_range;
      vects[i].norm();
    }
    else {
      vects[i].set(FLOAT_UNDEFINED);
    }
  }
  
  VECT last(0);
  for(i = 0; i < points; i++) {    
    if(vects[i].x != FLOAT_UNDEFINED) {
      last = vects[i];      
      break;
    }
  }
  
  if(i == points) {
    // return some default normal
    return(VECT3DF(0,-1,0));
  }
  
  VECT normal(0);
  for(i = i+1; i < points; i++) {
    if(vects[i].x != FLOAT_UNDEFINED) {
      VECT3DF tmp = vect_mult(last, vects[i]);
      tmp.norm();
      normal += tmp;
      last = vects[i];
    }
  }
  
  normal.norm();
  return(normal);
}
Example #16
0
int main()
{
	ofstream output; 
	ifstream input_train ; 
	ifstream input_test ; 
	
	//ofstream save ;


	output.open("ex.txt"); 
	input_train.open("trainSet");
	
	
	//save.open("Net.ini");

	YVector normal(2);
	YVector normal_out(1);
	normal = 1;
	normal_out = 1;

	// e deve avere la dimensione di una riga di Y

	YVector Yp(1),e(1),X(2),Y(1);
	double vecw;
	int i;
	
	RFNet prima;

	
	//  rfwr('Init',ID,2,1,0,0,0,1e-7,50,[1;1],[1],'rfwr_test');
	prima.Init(2,1,0,0,0,0.0000001,50,normal,normal_out,"rfwr_test");


	
	//rfwr('Change',ID,'init_D',[25 0.01; 0.01 25]); 
	double el[] = {25, 0.01, 0.01,25};
	prima.SetInitD (2,2,el);

	//rfwr('Change',ID,'init_alpha',ones(2)*250);     
	YMatrix a(2,2);
	a = 250 * ones(2);
	prima.SetInitAlpha (a);

	
	//rfwr('Change',ID,'w_gen',0.2);                  
	prima.SetWGen (0.2);
	
	//rfwr('Change',ID,'meta',1);                     
//	prima.SetMeta (1);

	//rfwr('Change',ID,'meta_rate',250);
//	prima.SetMetaRate (250);


	for (i = 1; i <=N*20; i++) {

	input_train >> X(1);
	input_train >> X(2);
	
	input_train >> Y(1);		
	
	
	vecw = prima.Train (X,Y,Yp,e);

	// Per stampare il risulato del train

//      cout << vecw << endl;
//      output << vecw << endl;
		
//		cout << Yp << endl;
//		output << Yp << endl;

//		cout << i << endl;
		
	
	}

	input_train.close();

	input_test.open("testSet");


	for (i = 1; i <=1681; i++) {

	input_test >> X(1);
	input_test >> X(2);

	input_test >> Y(1);

	//[yp,w]=rfwr('Predict',ID,Xt(i,:)',0.001);
	prima.Simulate (X,0.001,Yp);

// Per stampare il risultato del predict

//		cout << Yp << endl;
//		output << Yp << endl;

	outIni(Yp,output);
	}


	
//	prima.print(output); //****
//	prima.SaveNet (save);


	output.close();

	input_test.close();
//	save.close();

	return 0;

}
Example #17
0
static void makeFastFace(TileSpec tile, u16 li0, u16 li1, u16 li2, u16 li3,
		v3f p, v3s16 dir, v3f scale, u8 light_source, std::vector<FastFace> &dest)
{
	// Position is at the center of the cube.
	v3f pos = p * BS;

	float x0 = 0.0;
	float y0 = 0.0;
	float w = 1.0;
	float h = 1.0;

	v3f vertex_pos[4];
	v3s16 vertex_dirs[4];
	getNodeVertexDirs(dir, vertex_dirs);

	v3s16 t;
	u16 t1;
	switch (tile.rotation)
	{
	case 0:
		break;
	case 1: //R90
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[3];
		vertex_dirs[3] = vertex_dirs[2];
		vertex_dirs[2] = vertex_dirs[1];
		vertex_dirs[1] = t;
		t1=li0;
		li0=li3;
		li3=li2;
		li2=li1;
		li1=t1;
		break;
	case 2: //R180
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[2];
		vertex_dirs[2] = t;
		t = vertex_dirs[1];
		vertex_dirs[1] = vertex_dirs[3];
		vertex_dirs[3] = t;
		t1  = li0;
		li0 = li2;
		li2 = t1;
		t1  = li1;
		li1 = li3;
		li3 = t1;
		break;
	case 3: //R270
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[1];
		vertex_dirs[1] = vertex_dirs[2];
		vertex_dirs[2] = vertex_dirs[3];
		vertex_dirs[3] = t;
		t1  = li0;
		li0 = li1;
		li1 = li2;
		li2 = li3;
		li3 = t1;
		break;
	case 4: //FXR90
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[3];
		vertex_dirs[3] = vertex_dirs[2];
		vertex_dirs[2] = vertex_dirs[1];
		vertex_dirs[1] = t;
		t1  = li0;
		li0 = li3;
		li3 = li2;
		li2 = li1;
		li1 = t1;
		y0 += h;
		h *= -1;
		break;
	case 5: //FXR270
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[1];
		vertex_dirs[1] = vertex_dirs[2];
		vertex_dirs[2] = vertex_dirs[3];
		vertex_dirs[3] = t;
		t1  = li0;
		li0 = li1;
		li1 = li2;
		li2 = li3;
		li3 = t1;
		y0 += h;
		h *= -1;
		break;
	case 6: //FYR90
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[3];
		vertex_dirs[3] = vertex_dirs[2];
		vertex_dirs[2] = vertex_dirs[1];
		vertex_dirs[1] = t;
		t1  = li0;
		li0 = li3;
		li3 = li2;
		li2 = li1;
		li1 = t1;
		x0 += w;
		w *= -1;
		break;
	case 7: //FYR270
		t = vertex_dirs[0];
		vertex_dirs[0] = vertex_dirs[1];
		vertex_dirs[1] = vertex_dirs[2];
		vertex_dirs[2] = vertex_dirs[3];
		vertex_dirs[3] = t;
		t1  = li0;
		li0 = li1;
		li1 = li2;
		li2 = li3;
		li3 = t1;
		x0 += w;
		w *= -1;
		break;
	case 8: //FX
		y0 += h;
		h *= -1;
		break;
	case 9: //FY
		x0 += w;
		w *= -1;
		break;
	default:
		break;
	}

	for(u16 i=0; i<4; i++)
	{
		vertex_pos[i] = v3f(
				BS/2*vertex_dirs[i].X,
				BS/2*vertex_dirs[i].Y,
				BS/2*vertex_dirs[i].Z
		);
	}

	for(u16 i=0; i<4; i++)
	{
		vertex_pos[i].X *= scale.X;
		vertex_pos[i].Y *= scale.Y;
		vertex_pos[i].Z *= scale.Z;
		vertex_pos[i] += pos;
	}

	f32 abs_scale = 1.0;
	if     (scale.X < 0.999 || scale.X > 1.001) abs_scale = scale.X;
	else if(scale.Y < 0.999 || scale.Y > 1.001) abs_scale = scale.Y;
	else if(scale.Z < 0.999 || scale.Z > 1.001) abs_scale = scale.Z;

	v3f normal(dir.X, dir.Y, dir.Z);

	u8 alpha = tile.alpha;

	dest.push_back(FastFace());

	FastFace& face = *dest.rbegin();

	face.vertices[0] = video::S3DVertex(vertex_pos[0], normal,
			MapBlock_LightColor(alpha, li0, light_source),
			core::vector2d<f32>(x0+w*abs_scale, y0+h));
	face.vertices[1] = video::S3DVertex(vertex_pos[1], normal,
			MapBlock_LightColor(alpha, li1, light_source),
			core::vector2d<f32>(x0, y0+h));
	face.vertices[2] = video::S3DVertex(vertex_pos[2], normal,
			MapBlock_LightColor(alpha, li2, light_source),
			core::vector2d<f32>(x0, y0));
	face.vertices[3] = video::S3DVertex(vertex_pos[3], normal,
			MapBlock_LightColor(alpha, li3, light_source),
			core::vector2d<f32>(x0+w*abs_scale, y0));

	face.tile = tile;
}
Example #18
0
void YPlus::execute()
{
  Mesh& mesh = this->mesh();

  // Geometry data
  const Field& coords = mesh.geometry_fields().coordinates();
  const Uint nb_nodes = coords.size();
  const Uint dim = coords.row_size();

  // Velocity data
  const Field& velocity_field = common::find_component_recursively_with_tag<Field>(mesh, options().value<std::string>("velocity_tag"));
  const auto velocity_dict_handle = Handle<Dictionary const>(velocity_field.parent());
  cf3_assert(velocity_dict_handle != nullptr);
  const Dictionary& velocity_dict = *velocity_dict_handle;
  const Uint vel_offset = velocity_field.descriptor().offset("Velocity");

  // initialize if needed
  auto volume_node_connectivity = Handle<NodeConnectivity>(mesh.get_child("volume_node_connectivity"));
  if(m_normals.empty())
  {
    // Node-to-element connectivity for the volume elements
    volume_node_connectivity = mesh.create_component<NodeConnectivity>("volume_node_connectivity");
    std::vector< Handle<Entities const> > volume_entities;
    for(const mesh::Elements& elements : common::find_components_recursively_with_filter<mesh::Elements>(mesh, IsElementsVolume()))
    {
      volume_entities.push_back(elements.handle<Entities const>());
    }
    volume_node_connectivity->initialize(nb_nodes, volume_entities);

    mesh.geometry_fields().create_field("wall_velocity_gradient_nodal").add_tag("wall_velocity_gradient_nodal");
    Dictionary& wall_P0 = *mesh.create_component<DiscontinuousDictionary>("wall_P0");

    m_normals.clear();
    for(const Handle<Region>& region : regions())
    {
      for(mesh::Elements& wall_entity : common::find_components_recursively_with_filter<mesh::Elements>(*region, IsElementsSurface()))
      {
        const Uint nb_elems = wall_entity.size();
        const auto& geom_conn = wall_entity.geometry_space().connectivity();
        const ElementType& etype = wall_entity.element_type();
        const Uint element_nb_nodes = etype.nb_nodes();

        m_normals.push_back(std::vector<RealVector>(nb_elems, RealVector(dim)));

        RealMatrix elem_coords(element_nb_nodes, dim);
        for(Uint elem_idx = 0; elem_idx != nb_elems; ++elem_idx)
        {
          const Connectivity::ConstRow conn_row = geom_conn[elem_idx];
          fill(elem_coords, coords, conn_row);
          RealVector normal(dim);
          etype.compute_normal(elem_coords, m_normals.back()[elem_idx]);
          m_normals.back()[elem_idx] /= m_normals.back()[elem_idx].norm();
        }

        wall_entity.create_component<FaceConnectivity>("wall_face_connectivity")->initialize(*volume_node_connectivity);
        wall_entity.create_space("cf3.mesh.LagrangeP0."+wall_entity.element_type().shape_name(),wall_P0);
      }
    }
    wall_P0.build();         // to tell the dictionary that all spaces have been added
    mesh.update_structures(); // to tell the mesh there is a new dictionary added manually
    wall_P0.create_field("wall_velocity_gradient").add_tag("wall_velocity_gradient");
  }

  // Create the y+ field in the geometry dictionary
  if(common::find_component_ptr_with_tag(mesh.geometry_fields(), "yplus") == nullptr)
  {
    mesh.geometry_fields().create_field("yplus").add_tag("yplus");
  }

  // Compute shear stress
  Uint surface_idx = 0;
  Dictionary& wall_P0 = *Handle<mesh::Dictionary>(mesh.get_child_checked("wall_P0"));
  Field& wall_velocity_gradient_field = *Handle<Field>(wall_P0.get_child_checked("wall_velocity_gradient"));
  for(const Handle<Region>& region : regions())
  {
    for(const mesh::Elements& elements : common::find_components_recursively_with_filter<mesh::Elements>(*region, IsElementsSurface()))
    {
      const Uint nb_elements = elements.geometry_space().connectivity().size();
      cf3_assert(elements.element_type().nb_faces() == 1);
      const auto& face_connectivity = *Handle<FaceConnectivity const>(elements.get_child_checked("wall_face_connectivity"));
      const auto& wall_conn = elements.space(wall_P0).connectivity();
      for(Uint surface_elm_idx = 0; surface_elm_idx != nb_elements; ++surface_elm_idx)
      {
        if(face_connectivity.has_adjacent_element(surface_elm_idx, 0))
        {
          const Uint wall_field_idx = wall_conn[surface_elm_idx][0];
          // Get the wall normal vector
          const RealVector& normal = m_normals[surface_idx][surface_elm_idx];
          RealVector3 normal3;
          normal3[0] = normal[0];
          normal3[1] = normal[1];
          normal3[2] = dim == 3 ? normal[2] : 0.;

          // The connected volume element
          NodeConnectivity::ElementReferenceT connected = face_connectivity.adjacent_element(surface_elm_idx, 0);
          const mesh::Entities& volume_entities = *face_connectivity.node_connectivity().entities()[connected.first];
          const Uint volume_elem_idx = connected.second;
          const auto& velocity_conn = volume_entities.space(velocity_dict).connectivity();
          const auto& velocity_sf = volume_entities.space(velocity_dict).shape_function();
          const auto& geom_conn = volume_entities.geometry_space().connectivity();
          const ElementType& volume_etype = volume_entities.element_type();

          const Uint nb_vel_nodes = velocity_sf.nb_nodes();
          const RealVector centroid_mapped_coord = 0.5*(velocity_sf.local_coordinates().colwise().minCoeff() + velocity_sf.local_coordinates().colwise().maxCoeff());
          RealMatrix elem_coords(geom_conn.row_size(), dim);
          fill(elem_coords, coords, geom_conn[volume_elem_idx]);

          RealVector tangential_velocity(nb_vel_nodes); // For every node, the component of the velocity tangential to the wall
          RealVector3 v3;
          for(Uint i = 0; i != nb_vel_nodes; ++i)
          {
            Eigen::Map<RealVector const> v(&velocity_field[velocity_conn[volume_elem_idx][i]][vel_offset], dim);
            v3[0] = v[0];
            v3[1] = v[1];
            v3[2] = dim == 3 ? v[2] : 0.;
            tangential_velocity[i] = v3.cross(normal3).norm();
          }
          wall_velocity_gradient_field[wall_field_idx][0] = fabs((volume_etype.jacobian(centroid_mapped_coord, elem_coords).inverse() * velocity_sf.gradient(centroid_mapped_coord) * tangential_velocity).dot(-normal));
        }
      }
      ++surface_idx;
    }
  }

  wall_velocity_gradient_field.synchronize();

  // Compute a nodal version of the wall velocity gradient
  const auto& wall_node_connectivity = *Handle<NodeConnectivity>(mesh.get_child_checked("wall_node_connectivity"));
  Field& wall_velocity_gradient_field_nodal = *Handle<Field>(mesh.geometry_fields().get_child_checked("wall_velocity_gradient_nodal"));
  for(Uint node_idx = 0; node_idx != nb_nodes; ++node_idx)
  {
    Uint nb_connected_elems = 0;
    wall_velocity_gradient_field_nodal[node_idx][0] = 0;
    for(const NodeConnectivity::ElementReferenceT elref : wall_node_connectivity.node_element_range(node_idx))
    {
      const Uint wall_field_idx = wall_node_connectivity.entities()[elref.first]->space(wall_P0).connectivity()[elref.second][0];
      wall_velocity_gradient_field_nodal[node_idx][0] += wall_velocity_gradient_field[wall_field_idx][0];
      ++nb_connected_elems;
    }
    if(nb_connected_elems != 0)
    {
      wall_velocity_gradient_field_nodal[node_idx][0] /= static_cast<Real>(nb_connected_elems);
    }
  }


  // Set Yplus
  Field& yplus_field = *Handle<Field>(mesh.geometry_fields().get_child_checked("yplus"));
  const Field& wall_distance_field = *Handle<Field>(mesh.geometry_fields().get_child_checked("wall_distance"));
  const auto& node_to_wall_element = *Handle<common::Table<Uint>>(mesh.get_child_checked("node_to_wall_element"));
  const Real nu = physical_model().options().value<Real>("kinematic_viscosity");
  for(Uint node_idx = 0; node_idx != nb_nodes; ++node_idx)
  {
    yplus_field[node_idx][0] = 0;
    if(node_to_wall_element[node_idx][0] != 0)
    {
      const Entities& wall_entities = *wall_node_connectivity.entities()[node_to_wall_element[node_idx][1]];
      const Uint wall_field_idx = wall_entities.space(wall_P0).connectivity()[node_to_wall_element[node_idx][2]][0];
      yplus_field[node_idx][0] = wall_distance_field[node_idx][0] * sqrt(nu*wall_velocity_gradient_field[wall_field_idx][0]) / nu;
    }
    else
    {
      yplus_field[node_idx][0] = wall_distance_field[node_idx][0] * sqrt(nu*wall_velocity_gradient_field_nodal[node_to_wall_element[node_idx][1]][0]) / nu;
    }
  }
}
void GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
#if 0 
	dFloat speed0;
	dFloat speed1;
	SpecialEffectStruct* currectEffect;

	// get the pointer to the special effect structure
	currectEffect = (SpecialEffectStruct *)NewtonMaterialGetMaterialPairUserData (material);

	// save the contact information
	NewtonMaterialGetContactPositionAndNormal (material, &currectEffect->m_position.m_x, &currectEffect->m_normal.m_x);
	NewtonMaterialGetContactTangentDirections (material, &currectEffect->m_tangentDir0.m_x, &currectEffect->m_tangentDir1.m_x);


	// Get the maximum normal speed of this impact. this can be used for positioning collision sound
	speed0 = NewtonMaterialGetContactNormalSpeed (material);
	if (speed0 > currectEffect->m_contactMaxNormalSpeed) {
		// save the position of the contact (for 3d sound of particles effects)
		currectEffect->m_contactMaxNormalSpeed = speed0;
	}

	// get the maximum of the two sliding contact speed
	speed0 = NewtonMaterialGetContactTangentSpeed (material, 0);
	speed1 = NewtonMaterialGetContactTangentSpeed (material, 1);
	if (speed1 > speed0) {
		speed0 = speed1;
	}

	// Get the maximum tangent speed of this contact. this can be used for particles(sparks) of playing scratch sounds 
	if (speed0 > currectEffect->m_contactMaxTangentSpeed) {
		// save the position of the contact (for 3d sound of particles effects)
		currectEffect->m_contactMaxTangentSpeed = speed0;
	}


#endif
	
	// read the table direction
//	dVector dir (tableDir);
//	dVector updir (TableDir);
//	NewtonBody* const body = NewtonJointGetBody0(contactJoint);
//	for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
//		dFloat speed;
//		dVector point;
//		dVector normal;	
//		dVector dir0;	
//		dVector dir1;	
//		dVector force;
//		NewtonMaterial* material;
//
//		material = NewtonContactGetMaterial (contact);
//		NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
//
//		// if the normal is vertical is large the say 40 degrees
//		if (fabsf (normal % upDir) > 0.7f) {
//			// rotate the normal to be aligned with the table direction
//			NewtonMaterialContactRotateTangentDirections (material, dir);
//		}
//	}


	NewtonBody* const body = NewtonJointGetBody0(contactJoint);
	for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
		dVector point(0.0f);
		dVector normal(0.0f);	
		dVector dir0(0.0f);	
		dVector dir1(0.0f);	
		dVector force(0.0f);

		NewtonMaterial* const material = NewtonContactGetMaterial (contact);

		NewtonMaterialGetContactForce (material, body, &force.m_x);
		NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
		NewtonMaterialGetContactTangentDirections (material, body, &dir0.m_x, &dir1.m_x);
		//dFloat speed = NewtonMaterialGetContactNormalSpeed(material);

		//speed = NewtonMaterialGetContactNormalSpeed(material);
		// play sound base of the contact speed.
		//
	}
}
Example #20
0
std::unique_ptr<Occluder> Occluder::bake(const std::vector<__m128>& vertices, __m128 refMin, __m128 refMax)
{
  assert(vertices.size() % 16 == 0);

  // Simple k-means clustering by normal direction to improve backface culling efficiency
  std::vector<__m128> quadNormals;
  for (auto i = 0; i < vertices.size(); i += 4)
  {
    auto v0 = vertices[i + 0];
    auto v1 = vertices[i + 1];
    auto v2 = vertices[i + 2];
    auto v3 = vertices[i + 3];

    quadNormals.push_back(normalize(_mm_add_ps(normal(v0, v1, v2), normal(v0, v2, v3))));
  }

  std::vector<__m128> centroids;
  std::vector<uint32_t> centroidAssignment;
  centroids.push_back(_mm_setr_ps(+1.0f, 0.0f, 0.0f, 0.0f));
  centroids.push_back(_mm_setr_ps(0.0f, +1.0f, 0.0f, 0.0f));
  centroids.push_back(_mm_setr_ps(0.0f, 0.0f, +1.0f, 0.0f));
  centroids.push_back(_mm_setr_ps(0.0f, -1.0f, 0.0f, 0.0f));
  centroids.push_back(_mm_setr_ps(0.0f, 0.0f, -1.0f, 0.0f));
  centroids.push_back(_mm_setr_ps(-1.0f, 0.0f, 0.0f, 0.0f));

  centroidAssignment.resize(vertices.size() / 4);

  bool anyChanged = true;
  for (int iter = 0; iter < 10 && anyChanged; ++iter)
  {
    anyChanged = false;

    for (auto j = 0; j < quadNormals.size(); ++j)
    {
      __m128 normal = quadNormals[j];

      __m128 bestDistance = _mm_set1_ps(-std::numeric_limits<float>::infinity());
      int bestCentroid = -1;
      for (int k = 0; k < centroids.size(); ++k)
      {
        __m128 distance = _mm_dp_ps(centroids[k], normal, 0x7F);
        if (_mm_comige_ss(distance, bestDistance))
        {
          bestDistance = distance;
          bestCentroid = k;
        }
      }

      if (centroidAssignment[j] != bestCentroid)
      {
        centroidAssignment[j] = bestCentroid;
        anyChanged = true;
      }
    }

    for (int k = 0; k < centroids.size(); ++k)
    {
      centroids[k] = _mm_setzero_ps();
    }

    for (int j = 0; j < quadNormals.size(); ++j)
    {
      int k = centroidAssignment[j];

      centroids[k] = _mm_add_ps(centroids[k], quadNormals[j]);
    }

    for (int k = 0; k < centroids.size(); ++k)
    {
      centroids[k] = normalize(centroids[k]);
    }
  }

  std::vector<__m128> orderedVertices;
  for (int k = 0; k < centroids.size(); ++k)
  {
    for (int j = 0; j < vertices.size() / 4; ++j)
    {
      if (centroidAssignment[j] == k)
      {
        orderedVertices.push_back(vertices[4 * j + 0]);
        orderedVertices.push_back(vertices[4 * j + 1]);
        orderedVertices.push_back(vertices[4 * j + 2]);
        orderedVertices.push_back(vertices[4 * j + 3]);
      }
    }
  }

  auto occluder = std::make_unique<Occluder>();

  __m128 invExtents = _mm_div_ps(_mm_set1_ps(1.0f), _mm_sub_ps(refMax, refMin));

  __m128 scalingX = _mm_set1_ps(2047.0f);
  __m128 scalingY = _mm_set1_ps(2047.0f);
  __m128 scalingZ = _mm_set1_ps(1023.0f);

  __m128 half = _mm_set1_ps(0.5f);

  for (size_t i = 0; i < orderedVertices.size(); i += 16)
  {
    for (auto j = 0; j < 4; ++j)
    {
      // Transform into [0,1] space relative to bounding box
      __m128 v0 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 0], refMin), invExtents);
      __m128 v1 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 4], refMin), invExtents);
      __m128 v2 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 8], refMin), invExtents);
      __m128 v3 = _mm_mul_ps(_mm_sub_ps(orderedVertices[i + j + 12], refMin), invExtents);

      // Transpose into [xxxx][yyyy][zzzz][wwww]
      _MM_TRANSPOSE4_PS(v0, v1, v2, v3);

      // Scale and truncate to int
      v0 = _mm_fmadd_ps(v0, scalingX, half);
      v1 = _mm_fmadd_ps(v1, scalingY, half);
      v2 = _mm_fmadd_ps(v2, scalingZ, half);

      __m128i X = _mm_cvttps_epi32(v0);
      __m128i Y = _mm_cvttps_epi32(v1);
      __m128i Z = _mm_cvttps_epi32(v2);

      // Pack to 11/11/10 format
      __m128i XYZ = _mm_or_si128(_mm_slli_epi32(X, 21), _mm_or_si128(_mm_slli_epi32(Y, 10), Z));

      occluder->m_vertexData.push_back(XYZ);
    }
  }

  occluder->m_refMin = refMin;
  occluder->m_refMax = refMax;

  __m128 min = _mm_set1_ps(+std::numeric_limits<float>::infinity());
  __m128 max = _mm_set1_ps(-std::numeric_limits<float>::infinity());

  for (size_t i = 0; i < orderedVertices.size(); ++i)
  {
    min = _mm_min_ps(vertices[i], min);
    max = _mm_max_ps(vertices[i], max);
  }

  // Set W = 1 - this is expected by frustum culling code
  min = _mm_blend_ps(min, _mm_set1_ps(1.0f), 0b1000);
  max = _mm_blend_ps(max, _mm_set1_ps(1.0f), 0b1000);

  occluder->m_boundsMin = min;
  occluder->m_boundsMax = max;

  occluder->m_center = _mm_mul_ps(_mm_add_ps(max, min), _mm_set1_ps(0.5f));

  return occluder;
}
Example #21
0
void Game::paintEvent(QPaintEvent *event)
{
	if(gameStarted)
	{
		///*** Setup Painter ***///
		QPainter painter(this);
		QBrush normal(painter.brush());
		QBrush manaBar(Qt::blue,Qt::SolidPattern);
		QBrush healthBar(Qt::green,Qt::SolidPattern);
		
		///*** Prevent painter from drawing if the game is over ***///
		if (gameOver) 
		{
			painter.end();
			QPainter painter(this);
		
			QFont font("Courier", 15, QFont::DemiBold);
			QFontMetrics fm(font);
			int textWidth = fm.width("Game Over!");
			int textWidth2 = fm.width("Press ESC to close window");

			painter.setFont(font);

			painter.translate(QPoint(400, 300));
			painter.drawText(-textWidth/2, 0, "Game Over!");
			painter.drawText(-textWidth2/2, 15, "Press ESC to close window");
			killTimer(timerId);
			std::cout << "BLAHBLAHBLAH\n";
		}
	
		///*** Draw Player ***///
		if(player->getType() == TANK)
		{
			if(player->getCanAttack())
				painter.drawImage(player->getLocation().x()-9,player->getLocation().y()-5,player->getAttackFrame());
			if(player->getAttacking())
				painter.drawImage(player->getLocation().x()-10,player->getLocation().y()-10,player->getManaAttackFrame());
		} else if(player->getType() == ASSN) {
			if(player->getCanAttack())
			{
				painter.drawImage(player->getLocation().x()-20,player->getLocation().y()-17,player->getAttackFrame());
			}
			if(player->bombSet)
				painter.drawImage(player->bomb->getLocation().x()-20,player->bomb->getLocation().y()-17,player->getManaAttackFrame());
		}
	
		///*** Draw Enemies ***///
		for(int i=0;i<drones.size();i++)
		{
			painter.setBrush(healthBar);
			painter.drawRect(drones.at(i)->getLocation().x()-15,drones.at(i)->getLocation().y()-20,drones.at(i)->getHealth(), 4);
			painter.setBrush(normal);
			painter.drawImage(drones.at(i)->getLocation().x()-15,drones.at(i)->getLocation().y()-15,drones.at(i)->getAttackFrame());
		}
		if(canSpawnArcher)
		{
			for(int i=0;i<archers.size();i++)
			{
				painter.setBrush(normal);
				painter.drawImage(archers.at(i)->getLocation().x()-15,archers.at(i)->getLocation().y()-15,archers.at(i)->getAttackFrame());
				if(archers.at(i)->getAttacking())
				{
					painter.setBrush(manaBar);
					painter.drawEllipse(archers.at(i)->bullet->getLocation().x(),archers.at(i)->bullet->getLocation().y(),10,10);

				}
			}
		}
	
		///*** Draw Status Bars ***///
		painter.setBrush(healthBar);
		painter.drawRect(5,5,player->getHealth(),10);
		painter.setBrush(normal);
		painter.drawRect(5,5,player->getMaxHealth(),10);
		painter.setBrush(manaBar);
		painter.drawRect(5,20,player->getMana(),10);
		painter.setBrush(normal);
		painter.drawRect(5,20,player->getMaxMana(),10);
	
		///*** Give player tips at appropriate levels ***///
		if (player->getXP() < 1) 
		{
			if(player->getType() == TANK)
			{	
				QFont font("Courier", 15, QFont::DemiBold);
				QFontMetrics fm(font);
				int textWidth = fm.width("Left Click to 'Enrage', then run into an enemy!");

				painter.setFont(font);

				painter.translate(QPoint(400, 300));
				painter.drawText(-textWidth/2, 0, "Left Click to 'Enrage', then run into an enemy!");
			}
			else if(player->getType() == ASSN)
			{	
				QFont font("Courier", 15, QFont::DemiBold);
				QFontMetrics fm(font);
				int textWidth = fm.width("Left Click to Slash enemies, but don't get too close!");

				painter.setFont(font);

				painter.translate(QPoint(400, 300));
				painter.drawText(-textWidth/2, 0, "Left Click to Slash enemies, but don't get too close!");
			}
		}
		if (player->getXP() == 5) 
		{
			QFont font("Courier", 15, QFont::DemiBold);
			QFontMetrics fm(font);
			int textWidth = fm.width("Level Up!\nRight Click for New Skill");

			painter.setFont(font);

			painter.translate(QPoint(400, 300));
			painter.drawText(-textWidth/2, 0, "Level Up! Right Click for New Skill");
			level=2;
			emit levelChanged(level);
			canSpawnArcher = true;
		}
		if (player->getXP() == 10)
		{
			QFont font("Courier", 15, QFont::DemiBold);
			QFontMetrics fm(font);
			int textWidth = fm.width("Level Up!");

			painter.setFont(font);

			painter.translate(QPoint(400, 300));
			painter.drawText(-textWidth/2, 0, "Level Up!");
			canSpawnArcher = true;
			level=3;
			emit levelChanged(level);
		}
	}
}
point boundaryLayers::createNewVertex
(
    const label bpI,
    const boolList& treatPatches,
    const List<direction>& patchVertex
) const
{
    const meshSurfaceEngine& mse = surfaceEngine();
    const labelList& bPoints = mse.boundaryPoints();
    const faceList::subList& bFaces = mse.boundaryFaces();
    const vectorField& pNormals = mse.pointNormals();
    const VRWGraph& pFaces = mse.pointFaces();
    const labelList& boundaryFacePatches = mse.boundaryFacePatches();
    const VRWGraph& pointPoints = mse.pointPoints();

    const meshSurfacePartitioner& mPart = surfacePartitioner();
    const VRWGraph& pPatches = mPart.pointPatches();

    const pointFieldPMG& points = mesh_.points();

    # ifdef DEBUGLayer
    Info << "Creating new vertex for boundary vertex " << bpI << endl;
    Info << "Global vertex label " << bPoints[bpI] << endl;
    # endif

    vector normal(vector::zero);
    scalar dist(VGREAT);
    const point& p = points[bPoints[bpI]];
    if( patchVertex[bpI] & EDGENODE )
    {
        # ifdef DEBUGLayer
        Info << "Vertex is on the border" << endl;
        # endif

        DynList<label> otherPatches;
        forAllRow(pPatches, bpI, patchI)
            if( !treatPatches[pPatches(bpI, patchI)] )
                otherPatches.appendIfNotIn
                (
                    pPatches(bpI, patchI)
                );

        if( otherPatches.size() == 1 )
        {
            //- vertex is on an edge
            # ifdef DEBUGLayer
            Info << "Vertex is on an edge" << endl;
            # endif
            vector v(vector::zero);

            forAllRow(pFaces, bpI, pfI)
            {
                const face& f = bFaces[pFaces(bpI, pfI)];
                const label patchLabel =
                    boundaryFacePatches[pFaces(bpI, pfI)];

                if( treatPatches[patchLabel] )
                {
                    normal += f.normal(points);
                }
                else
                {
                    v += f.normal(points);
                }
            }

            const scalar magV = mag(v) + VSMALL;
            v /= magV;

            normal -= (normal & v) * v;

            const scalar magN = mag(normal) + VSMALL;
            normal /= magN;

            forAllRow(pointPoints, bpI, ppI)
            {
                if( patchVertex[pointPoints(bpI, ppI)] )
                    continue;

                const vector vec = points[bPoints[pointPoints(bpI, ppI)]] - p;
                const scalar prod = 0.5 * mag(vec & normal);

                if( prod < dist )
                    dist = prod;
            }
        }
Example #23
0
bool UnitSphere::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
		const Matrix4x4& modelToWorld ) {
	// DONE: implement intersection code for UnitSphere, which is centred
	// on the origin.
	//
	// Your goal here is to fill ray.intersection with correct values
	// should an intersection occur.  This includes intersection.point,
	// intersection.normal, intersection.none, intersection.t_value.
	//
	// HINT: Remember to first transform the ray into object space
	// to simplify the intersection test.

	// transform the ray into object space
	Ray3D obj_ray;
	obj_ray.origin = worldToModel * ray.origin;
	obj_ray.dir = worldToModel * ray.dir;

	// referred to the solution at:
	// http://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-sphere-intersection

	// analytic solution
	Vector3D L = obj_ray.origin - Point3D(0, 0, 0);
	double a = obj_ray.dir.dot(obj_ray.dir);
	double b = 2 * obj_ray.dir.dot(L);
	double c = L.dot(L) - 1.0;

	double discriminant = b * b - 4 * a * c;
	double x, y, z, t_value, t_value1, t_value2;

	// no solution
	if(discriminant < 0)
	{
		return false;
	}

	// 1 solution
	if(discriminant == 0)
	{
		t_value = - 0.5 * b / a;
	}

	// 2 solutions
	if(discriminant > 0)
	{
		float q = (b > 0) ? -0.5 * (b + sqrt(discriminant)) : -0.5 * (b - sqrt(discriminant));
		t_value1 = q / a;
		t_value2 = c / q;
	}

	if (t_value1 < t_value2){
		t_value = t_value1;
	}
	else{
		t_value = t_value2;
	}

	if(ray.intersection.none || t_value < ray.intersection.t_value)
	{
		// 1. x componet of the intersection
		double x = obj_ray.origin[0] + t_value * obj_ray.dir[0];
		// 2. y componet of the intersection
		double y = obj_ray.origin[1] + t_value * obj_ray.dir[1];
		// 3. z componet of the intersection
		double z = obj_ray.origin[2] + t_value * obj_ray.dir[2];

		Point3D intersection = modelToWorld * Point3D(x, y, z);
		Vector3D normal(x, y, z);
		normal = transNorm(worldToModel, normal);
		normal.normalize();

		ray.intersection.point = intersection;
		ray.intersection.normal = normal;
		ray.intersection.mat = ray.intersection.mat;
		ray.intersection.t_value = t_value;
		ray.intersection.none = false;
		return true;
	}
}
        void PhysicalWorld::onGeometryCollision(void*   i_world,
                                                dGeomID i_geometry1,
                                                dGeomID i_geometry2)
        {
          // due to organisation i_geometry1 and i_geometry2 are not spaces.
          InternalMessage("Physic","PhysicalWorld::onGeometryCollision entering") ;

          if (! Collideable::canCollide(i_geometry1,i_geometry2))
            return ;

          // i_world is in fact a world.
          PhysicalWorld* world = static_cast<PhysicalWorld*>(i_world) ;

          if (!world)
          {
            return ;
          }

          // retreive usefull objects
          Collideable* collideable1
            = static_cast<Collideable*>(dGeomGetData(i_geometry1)) ;
          Collideable* collideable2
            = static_cast<Collideable*>(dGeomGetData(i_geometry2)) ;
          PhysicalObject* object1
            = collideable1 ? collideable1->getControler()->getControler<PhysicalObject>()
                           : NULL ;
          PhysicalObject* object2
            = collideable2 ? collideable2->getControler()->getControler<PhysicalObject>()
                           : NULL ;

          InternalMessage("Physic","PhysicalWorld::onGeometryCollision "
                          + (object1 ? Kernel::toString(object1->getObject()->getIdentifier()) : "no object1")
                          + " "
                          + (object2 ? Kernel::toString(object2->getObject()->getIdentifier()) : "no object2")
                          + (collideable1->isCollideableWith(collideable2) ? " collideable" : "not collideable")
                          ) ;

          if (object1 && object2)
          {
            // object positions
            Ogre::Vector3 object1_position ;
            Ogre::Vector3 object2_position ;

            const dReal* temp_position ;
            temp_position = dBodyGetPosition(object1->getBody()->id()) ;
            object1_position.x = temp_position[0] ;
            object1_position.y = temp_position[1] ;
            object1_position.z = temp_position[2] ;

            temp_position = dBodyGetPosition(object2->getBody()->id()) ;
            object2_position.x = temp_position[0] ;
            object2_position.y = temp_position[1] ;
            object2_position.z = temp_position[2] ;

            InternalMessage("Physic","PhysicalWorld::onGeometryCollision object positions "
                            + Ogre::StringConverter::toString(object1_position)
                            + ";"
                            + Ogre::StringConverter::toString(object2_position)) ;


            // calculate contact points
            int number_of_contacts = dCollide(i_geometry1,
                                              i_geometry2,
                                              maximum_contact_points,
                                              contact_points,
                                              sizeof(dContactGeom)) ;

            InformationMessage("Physic","number of contact points = " + Kernel::toString(number_of_contacts)) ;

            Ogre::Vector3 result(0,0,0) ;

            int real_number_of_contact_points = 0 ;
            Ogre::Vector3 average_contact_point(0,0,0) ;

            for (int contact_index = 0 ;
                 contact_index < number_of_contacts ;
                 ++contact_index)
            {

              // create contact joint
              dContact contact ;
              contact.surface.mode = dContactSoftCFM|dContactSoftERP|dContactBounce ;
              contact.surface.mu = Kernel::Parameters::getValue<float>("Physic","ContactMu") ;
              contact.surface.mu2 = 0 ;
              contact.surface.bounce = Kernel::Parameters::getValue<float>("Physic","ContactBounce") ;
              contact.surface.bounce_vel = Kernel::Parameters::getValue<float>("Physic","ContactBounceVelocity") ;
              contact.surface.soft_erp = 1 ;
              contact.surface.soft_cfm = 1 ;
              contact.surface.motion1 = 0 ;
              contact.surface.motion2 = 0 ;
              contact.surface.slip1 = 0 ;
              contact.surface.slip2 = 0 ;
              contact.geom = contact_points[contact_index] ;
              contact.fdir1[0] = 0 ;
              contact.fdir1[1] = 0 ;
              contact.fdir1[2] = 0 ;
              contact.fdir1[3] = 0 ;

              Ogre::Vector3 point(contact_points[contact_index].pos[0],
                                  contact_points[contact_index].pos[1],
                                  contact_points[contact_index].pos[2]) ;
              Ogre::Vector3 v1 = point - object1_position ;

              Ogre::Vector3 normal(contact_points[contact_index].normal[0],
                                   contact_points[contact_index].normal[1],
                                   contact_points[contact_index].normal[2]) ;

              float dot1 = v1.dotProduct(normal) ;

              if (dot1 < 0)
              {
                average_contact_point += point ;

                dJointID joint_id = dJointCreateContact(world->m_world->id(),
                                                        world->m_contact_group,
                                                        &contact) ;

                dJointAttach(joint_id,
                             object1->getBody()->id(),
                             object2->getBody()->id()) ;

                ++real_number_of_contact_points ;
              }
            }

            if (real_number_of_contact_points != 0)
            {
              average_contact_point = average_contact_point / real_number_of_contact_points ;

              // create a collision object
              Kernel::Object* collision_object = world->getObject()->createObject() ;
              collision_object->addTrait(new Model::Collision(
                                     collideable1->getControler()->getObject(),
                                     collideable2->getControler()->getObject())) ;
              collision_object->addTrait(new Model::Positionned(
                                           Model::Position::Meter(
                                               average_contact_point.x,
                                               average_contact_point.y,
                                               average_contact_point.z))) ;
            }

          }
        }
Example #25
0
//creates a Mesh with the appropriate values and returns its pointer
//calculates the rotation
Mesh*
Surfrev::createMesh() {
    Mesh* m = new Mesh(); //empty mesh
    float degrees = 360/slices;
    float angle = 0;
    vec3 Y(0,1,0);
    //inserts all points doing the rotation around the y-axis
    for(int i=0; i<slices; i++) {
        for(int j=0; j<points; j++) {
            vec3 v = rotation3D(Y,angle)*polyline[j];
            //cout<<"X = "<<v[0]<<" Y = "<<v[1] <<" Z = "<< v[2]<<endl;
            m->addVertex(v);
        }
        angle += degrees;
    }
    for(int i=0; i<(slices*points)-1; i++) {
        //finds normal for face that is going to be inserted to the mesh
        if(i<((slices*points)-(points+1))) {
            int mod = (1+i)%points;
            if(mod != 0) {
                /*cout<<"--------\nPoint: "<<i<<endl;
                cout<<"i+1: "<<i+1<<endl;
                cout<<"i+1+points: "<<i+1+points<<endl;
                cout<<"i+points: "<<i+points<<"\n"<<endl;
                cout<<(mod)<<" = Mod\n\n";*/
            }

            vec4 vert1(m->getVertex(i)[0],m->getVertex(i)[1],m->getVertex(i)[2],1);
            vec4 vert2(m->getVertex(i+1)[0],m->getVertex(i+1)[1],m->getVertex(i+1)[2],1);
            vec4 vert3(m->getVertex(i+points+1)[0],m->getVertex(i+points+1)[1],m->getVertex(i+points+1)[2],1);
            //check for final point that has  x = 0: triangle not square
            if(((i+2)%points) == 0 && m->getVertex(i+1)[0] == 0) {
                vert3[0] = m->getVertex(i+points)[0];
                vert3[1] = m->getVertex(i+points)[1];
                vert3[2] = m->getVertex(i+points)[2];
            }


            vec4 n = getNormal(vert1,vert2,vert3);
            vec3 normal(n[0],n[1],n[2]);

            if(mod != 0) {
                /*std::cout<<"Normal x = "<<normal[0]<<std::endl;
                std::cout<<"Normal y = "<<normal[1]<<std::endl;
                std::cout<<"Normal z = "<<normal[2]<<std::endl;*/
            }
            m->addNormal(normal);
            ////creates the faces, normal index is = i, always have 4 vertices for a face
            vec2 f1((double)i,(double)i);
            vec2 f2((double)i+1,(double)i);
            vec2 f3((double)points+i+1,(double)i);
            vec2 f4((double)points+i,(double)i);
            std::vector<vec2> face;
            face.push_back(f2);
            face.push_back(f1);
            face.push_back(f4);
            face.push_back(f3);


            if(mod != 0) {
                //cout<<"add face: "<<i<<endl;
                m->addFace(face);
            }
        }
        //case: do last face
        else {
            int mod = (1+i)%points;
            int k = (points*slices);
            /*cout<<"Else Point: "<<i<<endl;
            cout<<"i+1: "<<i+1<<endl;
            cout<<"i+1+points: "<<i+1+points-k<<endl;
            cout<<"i+points: "<<i+points-k<<"\n"<<endl;
            cout<<(mod)<<" = Mod\n\n";*/

            vec4 vert1L(m->getVertex(i)[0],m->getVertex(i)[1],m->getVertex(i)[2],1);
            vec4 vert2L(m->getVertex(i+1)[0],m->getVertex(i+1)[1],m->getVertex(i+1)[2],1);
            vec4 vert3L(m->getVertex(i+points+1-k)[0],m->getVertex(i+points+1-k)[1],m->getVertex(i+points+1-k)[2],1);
            if(((i+2)%points) == 0 && m->getVertex(i+1)[0] == 0) {
                vert3L[0] = m->getVertex(i+points-k)[0];
                vert3L[1] = m->getVertex(i+points-k)[1];
                vert3L[2] = m->getVertex(i+points-k)[2];
            }
            vec4 nL = getNormal(vert1L,vert2L,vert3L);
            vec3 normalL(nL[0],nL[1],nL[2]);

            /*std::cout<<"--------\nNormal x = "<<normal[0]<<std::endl;
            std::cout<<"Normal y = "<<normal[1]<<std::endl;
            std::cout<<"Normal z = "<<normal[2]<<std::endl;*/
            m->addNormal(normalL);
            //creates the faces, normal index is = i, always have 4 vertices for a face
            vec2 f1L((double)i,(double)i);
            vec2 f2L((double)i+1,(double)i);
            vec2 f3L((double)points+i+1-k,(double)i);
            vec2 f4L((double)points+i-k,(double)i);
            std::vector<vec2> faceL;
            faceL.push_back(f2L);
            faceL.push_back(f1L);
            faceL.push_back(f4L);
            faceL.push_back(f3L);


            if(mod != 0) {
                //cout<<"add face: "<<i<<endl;
                m->addFace(faceL);
            }
        }
    }
    //check if there is an opening at the top or bottom to cap or not
    if(polyline[0][0] != 0) {	//cap the bottom
        /*cout<<"X1 = "<<m->getVertex(0)[0]<<"  Y1 = "<<m->getVertex(0)[1]<<"  Z1 = "<<m->getVertex(points-1)[2]<<endl;
        cout<<"X2 = "<<m->getVertex((points))[0]<<"  Y2 = "<<m->getVertex((points))[1]<<"  Z2 = "<<m->getVertex((points))[2]<<endl;
        cout<<"X3 = "<<m->getVertex((points*2))[0]<<"  Y3 = "<<m->getVertex((points*2))[1]<<"  Z3 = "<<m->getVertex((points*2))[2]<<"\n"<<endl;*/

        vec4 vert1(m->getVertex(0)[0],m->getVertex(0)[1],m->getVertex(0)[2],1);
        vec4 vert2(m->getVertex(points)[0],m->getVertex(points)[1],m->getVertex(points)[2],1);
        vec4 vert3(m->getVertex(points*2)[0],m->getVertex(points*2)[1],m->getVertex(points*2)[2],1);
        vec4 n = getNormal(vert1,vert2,vert3);
        vec3 normal(n[0],n[1],n[2]);

        m->addNormal(normal);
        std::vector<vec2> face;
        for(int k=0; k<slices; k++) {
            vec2 f((double)points*k,(double)m->lastNorm());
            face.push_back(f);
        }
        m->addFace(face);
    }

    if(polyline[points-1][0] != 0) {	//cap the top
        /*cout<<"Top\nX1 = "<<m->getVertex(points-1)[0]<<"  Y1 = "<<m->getVertex(points-1)[1]<<"  Z1 = "<<m->getVertex(points-1)[2]<<endl;
        cout<<"X2 = "<<m->getVertex((points*2)-1)[0]<<"  Y2 = "<<m->getVertex((points*2)-1)[1]<<"  Z2 = "<<m->getVertex((points*2)-1)[2]<<endl;
        cout<<"X3 = "<<m->getVertex((points*3)-1)[0]<<"  Y3 = "<<m->getVertex((points*3)-1)[1]<<"  Z3 = "<<m->getVertex((points*3)-1)[2]<<"\n"<<endl;*/

        vec4 vert1(m->getVertex(points-1)[0],m->getVertex(points-1)[1],m->getVertex(points-1)[2],1);
        vec4 vert2(m->getVertex((points*2)-1)[0],m->getVertex((points*2)-1)[1],m->getVertex((points*2)-1)[2],1);
        vec4 vert3(m->getVertex((points*3)-1)[0],m->getVertex((points*3)-1)[1],m->getVertex((points*3)-1)[2],1);
        vec4 n = getNormal(vert1,vert2,vert3);
        n = n * (-1);
        vec3 normal(n[0],n[1],n[2]);

        m->addNormal(normal);
        std::vector<vec2> face;
        for(int k=1; k<slices+1; k++) {
            vec2 f((double)(points*k)-1,(double)m->lastNorm());
            face.push_back(f);
        }
        m->addFace(face);
    }


    return m;

}
Example #26
0
void Ball::animateBall(float interval)
{
	bool collision = false;

	Vector3 normal(0.0f, 0.0f, 0.0f);

	Vector3 ball;
	Vector3 step;

	mVelocity = add(mVelocity, scale(MOVE_FORCE / mMass * interval, mPushDirection));
	mVelocity.z -= GRAVITY * interval;

	/* collision detection */

	step = scale(interval, mVelocity);

	ball = mPos + step;

	/* check only fields near by the ball. check field under ball first!!! */
	QuadList list = getFieldSphereIntersection(ball, BALL_RADIUS);

	while (list.next())
	{
		Quad quad = *list;
		const Vector3* q = quad.mVertices;
		const Vector3& dir = quad.mNormals[0];

		Vector3 a1 = closestPointTriangle(ball, q[0], q[1], q[2]);
		Vector3 a2 = closestPointTriangle(ball, q[2], q[3], q[0]);

		float d1 = (ball - a1).len();
		float d2 = (ball - a2).len();

		Vector3 a = (d1 <= d2) ? a1 : a2;

		if ((a - ball).len() <= BALL_RADIUS)
		{
			/* dist = vector from ball center to quad */
			Vector3 dist = a - ball;
			float l = dist.len();

			/* move = vector to move the ball out of quad */
			Vector3 move = scale(-((mRadius - l) / l), dist);

			/* some rotation for a better look */
			Vector3 right = norm(cross(dir, step));
			Vector3 forward = norm(cross(right, dir));

			mAngularRate =
				scale(dot(sub(ball, mPos), forward) /
						(2.0f * M_PI * mRadius) * 360.0f / interval, right);

			ball = add(ball, move);

			normal = add(normal, move);
			collision = true;
		}
	}

	mPos = ball;

	normal = norm(normal);

	mHasBallHitGoal = false;

	/* contact to surface? */
	if (collision)
	{
		float vn = dot(mVelocity, normal);
		Vector3 rebound = scale(-(1 + ELASTICITY) * vn, normal);

		if (len(rebound) > 3.0f * JUMP_FORCE * interval)
		{
			/* collision was to havy */
			explodeBall();
		}
		else
		{
			mVelocity = add(mVelocity, rebound);

			if (mJump)
			{
				mVelocity = add(mVelocity, scale(JUMP_FORCE / mMass * interval, normal));
			}
		}

		int x = (int) floor(ball.x - sgLevel.origin.x);
		int y = (int) floor(ball.y - sgLevel.origin.y);

		if (x == sgLevel.finish.x && y == sgLevel.finish.y)
		{
			/* hit goal */
			mHasBallHitGoal = true;
		}
	}

	/***/

	/* damping */
	mVelocity = scale(DAMPING, mVelocity);

	Quaternion rotation = mkQuaternion(len(mAngularRate) * interval, mAngularRate);
	mOrientation = mulQuaternion(rotation, mOrientation);

	/* falling to infinity */
	if (mPos.z < -1.0f)
	{
		explodeBall();
	}

	/* reset through user */
	if (wasKeyPressed(KEY_ENTER))
	{
		explodeBall();
	}

	mPushDirection = Vector3(0.0f, 0.0f, 0.0f);
	mJump = false;
}
	void AddCollisionTreeMesh (DemoEntityManager* const scene)
	{
		// open the level data
		char fullPathName[2048];
		GetWorkingFileName ("playground.ngd", fullPathName);

		scene->LoadScene (fullPathName);

		// find the visual mesh and make a collision tree
		NewtonWorld* const world = scene->GetNewton();
		DemoEntity* const entity = scene->GetLast()->GetInfo();
		DemoMesh* const mesh = (DemoMesh*)entity->GetMesh();
		dAssert (mesh->IsType(DemoMesh::GetRttiType()));

		NewtonCollision* const tree = NewtonCreateTreeCollision(world, 0);
		NewtonTreeCollisionBeginBuild(tree);

		dFloat* const vertex = mesh->m_vertex;
		for (DemoMesh::dListNode* node = mesh->GetFirst(); node; node = node->GetNext()){
			DemoSubMesh* const subMesh = &node->GetInfo();
			unsigned int* const indices = subMesh->m_indexes;
			int trianglesCount = subMesh->m_indexCount;
			for (int i = 0; i < trianglesCount; i += 3) {

				dVector face[3];
				int index = indices[i + 0] * 3;
				face[0] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]);

				index = indices[i + 1] * 3;
				face[1] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]);

				index = indices[i + 2] * 3;
				face[2] = dVector (vertex[index + 0], vertex[index + 1], vertex[index + 2]);

				int matID = 0;
				//matID = matID == 2 ? 1 : 2 ;
				NewtonTreeCollisionAddFace(tree, 3, &face[0].m_x, sizeof (dVector), matID);
			}
		}
		NewtonTreeCollisionEndBuild (tree, 0);

		// add the collision tree to the collision scene
		void* const proxy = NewtonSceneCollisionAddSubCollision (m_sceneCollision, tree);

		// destroy the original tree collision
		NewtonDestroyCollision (tree);

		// set the parameter on the added collision share
		dMatrix matrix (entity->GetCurrentMatrix());
		NewtonCollision* const collisionTree = NewtonSceneCollisionGetCollisionFromNode (m_sceneCollision, proxy);

		NewtonSceneCollisionSetSubCollisionMatrix (m_sceneCollision, proxy, &matrix[0][0]);	
		NewtonCollisionSetUserData(collisionTree, mesh);

		// set the application level callback
#ifdef USE_STATIC_MESHES_DEBUG_COLLISION
		NewtonStaticCollisionSetDebugCallback (collisionTree, ShowMeshCollidingFaces);
#endif

		mesh->AddRef();

		scene->RemoveEntity (entity);

	#ifdef USE_TEST_ALL_FACE_USER_RAYCAST_CALLBACK
		// set a ray cast callback for all face ray cast 
		NewtonTreeCollisionSetUserRayCastCallback (collisionTree, AllRayHitCallback);
		dVector p0 (0,  100, 0, 0);
		dVector p1 (0, -100, 0, 0);
		dVector normal(0.0f);
		dLong id;
		dFloat parameter;
		parameter = NewtonCollisionRayCast (collisionTree, &p0[0], &p1[0], &normal[0], &id);
	#endif
	}
Example #28
0
void Role::decipher(QString command)
{
    QStringList arg=command.split(';');
    QStringList cardIDList;
    int targetID,targetArea;
    int sourceID,sourceArea;
    int cardID;
    int howMany;
    int hitRate;
    int i;
    int team,gem,crystal;

    Card*card;
    Player*player;
    Team *red=dataInterface->getRedTeam();
    Team *blue=dataInterface->getBlueTeam();
    QList<Card*> cards;
    QString msg;
    QString flag;
    QString cardName;
    QList<Player*>playerList=dataInterface->getPlayerList();
    ShowArea* showArea=gui->getShowArea();

    switch (arg[0].toInt())
    {
//回合开始
    case 3:
        targetID=arg[1].toInt();
        gui->logAppend("-----------------------------------");
        gui->logAppend(playerList[targetID]->getName()+tr("回合开始"));
        playerArea->setCurrentPlayerID(targetID);
        if(targetID!=myID)
        {
            isMyTurn=0;
            gui->setEnable(0);
        }
        else{
            isMyTurn=true;
            onceUsed=false;
            start=false;
            usedAttack=usedMagic=usedSpecial=false;
        }
        break;
//应战询问
    case 5:
        hitRate=arg[1].toInt();
        cardID=arg[2].toInt();
        targetID=arg[3].toInt();
        sourceID=arg[4].toInt();
        card=dataInterface->getCard(cardID);

        if(targetID!=myID)
        {
            gui->setEnable(0);
        }
        else
        {
            gui->setEnable(1);
            msg=playerList[sourceID]->getName()+tr("对")+tr("你")+tr("使用了")+card->getName();
            if (hitRate==2)
                return;

            if(hitRate==1)
                msg+=tr(",该攻击无法应战");

            sourcePlayerID=sourceID;
            gui->reset();
            tipArea->setMsg(msg);
            attacked(card->getElement(),hitRate);
        }
        break;
//弃牌询问
    case 7:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        flag=arg[3];
        msg=playerList[targetID]->getName()+tr("需要弃")+arg[2]+tr("张手牌");
        if(flag=="y")
            gui->logAppend(msg+tr("(明弃)"));
        else
            gui->logAppend(msg+tr("(暗弃)"));
        if(targetID!=myID)
        {
            gui->setEnable(0);
        }
        else
        {
            gui->setEnable(1);
            gui->reset();
            drop(howMany);
            tipArea->setMsg(tr("你需要弃")+arg[2]+tr("张手牌"));
        }
        break;
//摸牌
    case 9:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        gui->logAppend(playerList[targetID]->getName()+tr("摸取")+arg[2]+tr("张手牌"));

        if(targetID==myID)
        {
            cardIDList=arg[3].split(',');
            for(i=0;i<howMany;i++)
            {
                cardID=cardIDList[i].toInt();
                card=dataInterface->getCard(cardID);
                dataInterface->addHandCard(card);
            }
        }
        break;
//牌堆重洗
    case 10:
        gui->logAppend(tr("牌堆已重洗"));
        break;
//士气改变
    case 11:
        team=arg[1].toInt();
        howMany=arg[2].toInt();
        if(team)
        {
            red->setMorale(howMany);
            teamArea->update();
        }
        else
        {
            blue->setMorale(howMany);
            teamArea->update();
        }

        break;
//游戏结束
    case 12:
        team=arg[1].toInt();
        msg=tr("游戏结束,");
        if(team)
            msg+=tr("红队获胜");
        else
            msg+=tr("蓝队获胜");
        gui->logAppend(msg);
        tipArea->setMsg(msg);
        break;
//弃牌公告
    case 13:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        flag=arg[3];
        msg=playerList[targetID]->getName()+tr("丢弃了")+arg[2]+tr("张手牌");
        if(flag=="y")
        {
            cardIDList=arg[4].split(',');
            msg+=":";
            cards.clear();
            for(i=0;i<howMany;i++)
            {
                cardID=cardIDList[i].toInt();
                card=dataInterface->getCard(cardID);
                msg+=card->getName()+"-"+card->getProperty()+" ";
                cards<<card;
                dataInterface->removeHandCard(card);
            }
            showArea->showCards(cards);
        }
        gui->logAppend(msg);
        break;
//命中公告
    case 14:
        flag=arg[1];
        targetID=arg[3].toInt();
        sourceID=arg[4].toInt();
        if(flag=="1")
            msg=playerList[sourceID]->getName()+tr("未命中")+playerList[targetID]->getName();
        else
            msg=playerList[sourceID]->getName()+tr("命中")+playerList[targetID]->getName();
        gui->logAppend(msg);
        break;
//星石改变
    case 15:
        team=arg[1].toInt();
        gem=arg[2].toInt();
        crystal=arg[3].toInt();
        if(team)
        {
            red->setGem(gem);
            red->setCrystal(crystal);
            teamArea->update();
        }
        else
        {
            blue->setGem(gem);
            blue->setCrystal(crystal);
            teamArea->update();
        }
        break;
//星杯改变
    case 17:
        team=arg[1].toInt();
        howMany=arg[2].toInt();
        if(team)
        {
            red->setGrail(howMany);
            teamArea->update();
        }
        else
        {
            blue->setGrail(howMany);
            teamArea->update();
        }
        break;
//能量改变
    case 18:
        targetID=arg[1].toInt();
        gem=arg[2].toInt();
        crystal=arg[3].toInt();
        player=playerList.at(targetID);
        player->setGem(gem);
        player->setCrystal(crystal);
        playerArea->update();
        break;
//移牌通告
    case 19:
        howMany=arg[1].toInt();
        cardIDList=arg[2].split(",");
        sourceID=arg[3].toInt();
        sourceArea=arg[4].toInt();
        targetID=arg[5].toInt();
        targetArea=arg[6].toInt();
        if(sourceID!=-1)
        {
            switch(sourceArea)
            {
            case 4:
                player=playerList.at(sourceID);
                player->changeHandCardNum(-howMany);
                break;
            case 5:
                cardID=cardIDList[0].toInt();
                card=dataInterface->getCard(cardID);
                playerList[sourceID]->removeStatus(card);
                break;
            }
        }
        if(targetID!=-1)
        {
            switch(targetArea)
            {
            case 4:
                player=playerList.at(targetID);
                player->changeHandCardNum(howMany);
                break;
            case 5:
                cardID=cardIDList[0].toInt();
                card=dataInterface->getCard(cardID);
                cardName=card->getName();
                if(cardName==tr("中毒"))
                    playerList[targetID]->addStatus(0,card);
                if(cardName==tr("虚弱"))
                    playerList[targetID]->addStatus(1,card);
                if(cardName==tr("圣盾")||card->getSpecialityList().contains(tr("天使之墙")))
                    playerList[targetID]->addStatus(2,card);
                if(card->getType()=="attack"&&card->getProperty()=="幻")
                    playerList[targetID]->addStatus(3,card);
                break;
            }
        }
        playerArea->update();
        break;
//物伤通告
    case 20:
        targetID=arg[1].toInt();
        sourceID=arg[3].toInt();
        gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("造成了")+arg[2]+tr("点攻击伤害"));
        break;
//法术通告
    case 21:
        targetID=arg[1].toInt();
        sourceID=arg[3].toInt();
        gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("使用的")+arg[4]+tr("造成了")+arg[2]+tr("点法术伤害"));
        break;
//虚弱询问
    case 22:
        targetID=arg[1].toInt();
        gui->logAppend(tr("等待")+playerList[targetID]->getName()+tr("虚弱响应"));
        if(targetID!=myID)
        {
            gui->setEnable(0);
        }
        else
        {
            gui->setEnable(1);
            gui->reset();
            state=7;
            decisionArea->enable(0);
            decisionArea->enable(1);
            tipArea->setMsg(tr("你被虚弱了,选是跳过行动阶段,选否摸")+arg[2]+tr("张牌"));
        }
        break;
//虚弱结果
    case 24:
        targetID=arg[1].toInt();
        if(arg[2]=="0")
            gui->logAppend(playerList[targetID]->getName()+tr("选择跳过行动阶段"));
        else
            gui->logAppend(playerList[targetID]->getName()+tr("选择摸")+arg[3]+tr("张牌挣脱"));
        break;
//圣盾移除通告
    case 25:
        targetID=arg[1].toInt();
        gui->logAppend(playerList[targetID]->getName()+tr("的圣盾被移除"));
        break;
//魔弹询问
    case 26:
        int nextID;
        targetID=arg[1].toInt();
        sourceID=arg[2].toInt();
        howMany=arg[3].toInt();
        nextID=arg[4].toInt();
        gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("使用了魔弹,目前伤害为:")+arg[3]+tr("点"));
        if(targetID!=myID)
        {
            gui->setEnable(0);
        }
        else
        {
            gui->setEnable(1);
            msg=playerList[sourceID]->getName()+tr("对")+tr("你")+tr("使用了魔弹,目前伤害为:")+arg[3]+tr("点");
            gui->reset();
            tipArea->setMsg(msg);
            moDaned(nextID);
        }
        break;
//卡牌通告
    case 28:
        cardID=arg[1].toInt();
        targetID=arg[2].toInt();
        sourceID=arg[3].toInt();
        flag=arg[4];
        card=dataInterface->getCard(cardID);
        cardName=card->getName();
        if(flag=="1"){
            cards.clear();
            cards<<card;
            showArea->showCards(cards);
        }
        if(card->getElement()!="light")
            gui->logAppend(playerList[sourceID]->getName()+tr("对")+playerList[targetID]->getName()+tr("使用了")+cardName);
        else
            gui->logAppend(playerList[sourceID]->getName()+tr("使用圣光抵御"));
        break;
//行动阶段 flag 0-所有行动,1-攻击行动,2-法术行动,3-特殊行动,4-攻击或法术行动
    case 29:
        targetID=arg[1].toInt();
        flag=arg[2];
        if(targetID!=myID)
        {
            isMyTurn=0;
            gui->setEnable(0);
            actionFlag=-1;
        }
        else
        {
            gui->setEnable(1);
            if(flag=="0")
                normal();
            else if(flag=="1")
                attackAction();
            else if(flag=="2")
                magicAction();
            else if(flag=="4")
                attackOrMagic();
            if(arg[3]=="1")
                decisionArea->enable(3);
        }
        break;
//无法行动公告
    case 31:
        targetID=arg[1].toInt();
        msg=playerList[targetID]->getName()+tr("宣告无法行动");
        gui->logAppend(msg);
        break;
//治疗改变
    case 32:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        playerList[targetID]->setCrossNum(howMany);
        break;
//治疗询问
    case 33:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        flag=arg[3];
        gui->reset();
        if(targetID==myID)
            cure(playerList[myID]->getCrossNum(),howMany,flag.toInt());
        break;
//技能响应询问
    case 35:
        targetID=arg[1].toInt();
        flag=arg[2];
        msg=tr("等待")+playerList[targetID]->getName()+flag+tr("响应");
        gui->logAppend(msg);
        gui->reset();
        gui->setEnable(0);
        break;
//信息通告
    case 38:
        gui->logAppend(arg[1]);
        break;
//角色形态转换通知
    case 39:
        targetID=arg[1].toInt();
        flag=arg[2];
        if(flag=="0")
        {
            playerList[targetID]->setTap(0);
            msg=playerList[targetID]->getName()+tr("返回")+arg[3];
        }
        else
        {
            playerList[targetID]->setTap(1);
            msg=playerList[targetID]->getName()+tr("进入")+arg[3];
        }
        playerArea->update();
        gui->logAppend(msg);
        gui->reset();
        gui->setEnable(0);
        break;
//手牌上限变更通知
    case 40:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        playerList[targetID]->setHandCardsMax(howMany);
        playerArea->update();
        break;
//获得手牌通告
    case 41:
        targetID=arg[1].toInt();
        flag=arg[2];
        howMany=arg[3].toInt();
        gui->logAppend(playerList[targetID]->getName()+tr("获得")+arg[3]+tr("张手牌"));

        if(targetID==myID)
        {
            cardIDList=arg[4].split(',');
            for(i=0;i<howMany;i++)
            {
                cardID=cardIDList[i].toInt();
                card=dataInterface->getCard(cardID);
                dataInterface->addHandCard(card);
            }
        }
        break;
//额外行动询问
    case 42:
        targetID=arg[1].toInt();
        gui->logAppend(tr("等待")+playerList[targetID]->getName()+tr("额外行动响应"));
        if(targetID==myID)
        {
            gui->setEnable(1);
            addtionalAction();
        }
        else
            gui->setEnable(0);
        break;
//专属变更
    case 43:
        targetID=arg[1].toInt();
        msg=arg[2];
        playerList.at(targetID)->setSpecial(msg.toInt(),arg[3].toInt());
        playerArea->update();
        break;
//标记变更
    case 45:
        targetID=arg[1].toInt();
        flag=arg[2];
        howMany=arg[3].toInt();
        playerList.at(targetID)->setToken(flag.toInt(),howMany);
        playerArea->update();
        break;
//天使祝福
    case 750:
        targetID=arg[1].toInt();
        howMany=arg[2].toInt();
        msg=tr("等待")+playerList[targetID]->getName()+tr("天使祝福(给牌)响应");
        gui->logAppend(msg);
        if(targetID!=myID)
        {
            isMyTurn=0;
            gui->setEnable(0);
        }
        else
        {
            gui->setEnable(1);
            TianShiZhuFu(howMany);
        }
        break;
//魔爆冲击(弃牌)
    case 850:
        targetID=arg[1].toInt();
        msg=tr("等待")+playerList[targetID]->getName()+tr("魔爆冲击(弃牌)响应");
        gui->logAppend(msg);
        if(targetID!=myID)
        {
            isMyTurn=0;
            gui->setEnable(0);
        }
        else
        {
            gui->setEnable(1);
            MoBaoChongJi();
        }
        break;

    }
}
Example #29
0
individual_tPtr AddNormalMute(param_tPtr eps, individual_tPtr ind, int gen)
	 
{
  unsigned long	i = 1, PLength;
  
  
  if(NULL == eps)
	panic(A_FATAL, "AddNormalMute", "uninitialized parameter structure : %s : %d",
		  __FILE__, __LINE__);
  
  if(NULL == ind)
	panic(A_FATAL, "AddNormalMute", "uninitialized individual : %s : %d",__FILE__, __LINE__);
  
  switch(gen) {
	
  case X:
	if(TRUE == CorrField[X])
	  return(_AddCorrNormalXMute(eps, ind));
	else
	  return(_AddNormalXMute(eps, ind));
	
  case ALPHA:
	for(i = 1; i <= inGetAlphaLength(ind); i++) {
	  inAssignAlphaComponent(i,
							 AlphaTRANSFORM(eps, ind, inGetAlphaComponent(i, ind),
											inGetAlphaComponent(i, ind) +
											eps->Beta * normal(1.0)), 
							 ind);
	}
	return(ind);
	
  case P:
	if(0 == (PLength = inGetPLength(ind)))
	  panic(A_WARN, "AddNormalMute", "zero dimensional p vector : %s : %d",
			__FILE__, __LINE__);
	
	if(1 == PLength)
	  inAssignPComponent(1,
						 PTRANSFORM(eps, ind, inGetPComponent(1, ind),
									inGetPComponent(1,ind) +
									eps->GamOne * normal(1.0)), 
						 ind);
	else {
	  for(i = 1; i <= inGetPLength(ind); i++) {
		inAssignPComponent(i, 
						   PTRANSFORM(eps, ind, inGetPComponent(i, ind),
									  inGetPComponent(i, ind) +
									  eps->GamLcl * normal(1.0)), 
						   ind);
	  }
	}
	return(ind);
	
  case D:
  case SIGMA:
  default:
	panic(A_FATAL, "AddNormalMute", "unallowed gen for the additiv normal mutation scheme : %s : %d",
		  __FILE__, __LINE__);
  }
  
  return(ind);
}
Example #30
0
void PlayerBehaviourAir::resolve(const sf::Vector3f& manifold, CollisionWorld::Body* other)
{
    switch (other->getType())
    {
    case CollisionWorld::Body::Type::Water:
        setBehaviour<PlayerBehaviourWater>();
        break;
    case CollisionWorld::Body::Type::Solid:
    case CollisionWorld::Body::Type::Block:
        move(sf::Vector2f(manifold.x, manifold.y) * manifold.z);
        if (manifold.y * manifold.z < 0) //contact is below so must be standing on something
        {
            setVelocity({ getVelocity().x, 0.f });
            setBehaviour<PlayerBehaviourGround>();

            Event playerEvent;
            playerEvent.type = Event::Player;
            playerEvent.player.playerId = Category::None;
            playerEvent.player.action = Event::PlayerEvent::Landed;
            raiseEvent(playerEvent);
        }
        break;
    case CollisionWorld::Body::Type::Player:
    {
        move(sf::Vector2f(manifold.x, manifold.y) * manifold.z);
        auto vel = getVelocity();
        if (manifold.x != 0) //players colliding sideways in air
        {
            setVelocity({ -vel.x, vel.y });
        }
        else if (manifold.y * manifold.z < 0)
        {
            //bounce off players below
            setVelocity({ vel.x, -vel.y * getFriction() });
        }
    }
    break;
    case CollisionWorld::Body::Type::Npc:
    {
        //move(sf::Vector2f(manifold.x, manifold.y) * manifold.z);

        ////bounce off NPCs
        //auto vel = getVelocity();
        //if (manifold.x != 0)
        //    vel.x = -vel.x;
        //if (manifold.y != 0)
        //    vel.y = -vel.y;

        //vel *= getFriction();
        //setVelocity(vel);
        kill();
    }
    break;
    case CollisionWorld::Body::Item:
    {
        Event e;
        e.node.action = Event::NodeEvent::KilledNode;
        e.node.type = getParentCategory();
        e.node.target = Category::Item;
        e.type = Event::Node;
        raiseEvent(e);
    }
        break;
    case CollisionWorld::Body::FreeForm:
    {
        if (other->getParentCategory() == Category::HatCarried) break;

        sf::Vector2f normal(manifold.x, manifold.y);
        move(normal * manifold.z);
        setVelocity(Util::Vector::reflect(getVelocity() * getFriction(), normal));
    }
        break;
    default: break;
    }
}