//---------------------------------------------------------------------------
bool CMesh::InsideTri(Vec3D<>& p, Vec3D<>& v0, Vec3D<>& v1, Vec3D<>& v2)
//---------------------------------------------------------------------------
{// True if point p projects to within triangle (v0;v1;v2)

    Vec3D<> xax = (v1-v0).Normalized();
    Vec3D<> zax = ((v2-v0).Cross(xax)).Normalized();
    Vec3D<> yax = zax.Cross(xax).Normalized();

    Vec3D<> p0(0,0,1);
    Vec3D<> p1((v1-v0).Dot(xax),(v1-v0).Dot(yax),1);
    Vec3D<> p2((v2-v0).Dot(xax),(v2-v0).Dot(yax),1);
    Vec3D<> pt((p-v0).Dot(xax),(p-v0).Dot(yax),1);

    vfloat d0 = Det(p0,p1,pt);
    vfloat d1 = Det(p1,p2,pt);
    vfloat d2 = Det(p2,p0,pt);

    if (d0<=0 && d1<=0 && d2<=0)
        return true;
    if (d0>=0 && d1>=0 && d2>=0)
        return true;

    return false;

}

 template<typename T2> Vec3D<T2> convert()
 {
    Vec3D<T2> ret;
    ret.at(0) =  static_cast<T2>(this->at(0));
    ret.at(1) =  static_cast<T2>(this->at(1));
    ret.at(2) =  static_cast<T2>(this->at(2));
 }

// For the description of the algorithm, see manual.tex
bool Reorient::calcRotationAngles(double &angle1, double &angle2) {
  if (gonioAxis.at(0).isNull() || gonioAxis.at(1).isNull()) return false;
  Vec3D nfrom = fromNormal();
  if (nfrom.isNull()) return false;
  Vec3D nto = toNormal();
  if (nto.isNull()) return false;

  // Calculates a line u1+lambda*u2 in 3d-space, that is the intersection of the
  // two planes with normal Vector
  // gonioAxis[0] and gonioAxis[1] and that contain nFrom and nTo, respectively
  Vec3D u1, u2;
  if (!calcLine(nfrom, nto, u1, u2)) return false;


  double score = -1;
  // check the up to two points of intersection of the line with the unit sphere
  foreach (Vec3D v, calcPossibleIntermediatePositions(u1, u2)) {
    double aChi=calcRotationAngle(nfrom, v, gonioAxis.at(0));
    double aPhi=calcRotationAngle(v, nto, gonioAxis.at(1));
    if ((score<0) || (score > (aChi*aChi+aPhi*aPhi))) {
      angle1 = aChi;
      angle2 = aPhi;
      score = (aChi*aChi+aPhi*aPhi);
    }
  }

void CP_Mesh::Draw(Vec3D<>* Envelope)
{
	glPushMatrix();
//	glScalef(MaxVal,MaxVal, MaxVal);

	glColor4d(R, G, B, alpha);
	Vec3D<> Min = ThisMesh.GetBBMin();
	Vec3D<> Size = ThisMesh.GetBBSize();
	Vec3D<> v1(X, Y, Z);
	
	if (Envelope){
		Vec3D<> ScaleFacV = Vec3D<>(dX, dY, dZ).Scale(Envelope->ScaleInv(Size)); //= WS/Size
		vfloat MinScaleFac = ScaleFacV.Min();
		v1 = v1.Scale(*Envelope);

		glTranslated(v1.x, v1.y, v1.z);
		glScaled(MinScaleFac, MinScaleFac, MinScaleFac);
	}
	else {
		glTranslated(v1.x, v1.y, v1.z);
		glScaled(dX/Size.x, dY/Size.y, dZ/Size.z);
	}
	glTranslated(-Min.x, -Min.y, -Min.z);

	ThisMesh.Draw(false, true, true, true);

	glPopMatrix();
}

bool toxi::geom::AABB::intersectsBox( AABB & box )
{
	Vec3D t = box.sub(x, y, z);
	return toxi::math::MathUtils::abs( t.getX() ) <= ( extent.getX() + box.extent.getX() )
		&& toxi::math::MathUtils::abs( t.getY() ) <= ( extent.getZ() + box.extent.getY() )
		&& toxi::math::MathUtils::abs( t.getY() ) <= ( extent.getY() + box.extent.getZ() );
}

Particle PlaneParticleEmitter::newParticle(int anim, int time)
{
    Particle p;
	// TODO: maybe evaluate these outside the spawn function, since they will be common for a given frame?
	float w = sys->areal.getValue(anim, time) * 0.5f;
	float l = sys->areaw.getValue(anim, time) * 0.5f;
	float spd = sys->speed.getValue(anim, time);
	float var = sys->variation.getValue(anim, time);

	p.pos = sys->pos + Vec3D(randfloat(-l,l), 0, randfloat(-w,w));
	p.pos = sys->parent->mat * p.pos;

	Vec3D dir = sys->parent->mrot * Vec3D(0,1,0);
	p.down = Vec3D(0,-1.0f,0); // dir * -1.0f;
	//p.speed = dir.normalize() * randfloat(spd1,spd2);   // ?
	p.speed = dir.normalize() * spd * (1.0f+randfloat(-var,var));

	p.life = 0;
	p.maxlife = sys->lifespan.getValue(anim, time);

	p.origin = p.pos;

	p.tile = randint(0, sys->rows*sys->cols-1);
	return p;
}

void TShadowManager::refreshTerrainTri()
{
	std::vector< int >& triVec = FuCShadowModify::getTriIDVec();

	triVec.clear();

	FnTriangle tT;
	tT.Object( curLevel->getFlyTerrain().Object() , 0);

	Vec3D actorPos = m_player->getPosition();
	int nList = tT.GetTriangleNumber();

	int tri[3];
	float pos[16];

	float r2 = gShadowMaxRadius * gShadowMaxRadius;

	for (int i = 0; i < nList; i++)
	{
		tT.GetTopology( i , tri);
		tT.GetVertex(tri[0], pos);
		float dx = actorPos.x() - pos[0];
		float dy = actorPos.y() - pos[1];
		if ( dx * dx + dy * dy < r2 )
			triVec.push_back( i );
	}
}

toxi::geom::Triangle3D toxi::geom::Triangle3D::createEquilateralFrom( Vec3D & a, Vec3D & b )
{
    Vec3D c = a.interpolateTo( b, 0.5 );
    Vec3D dir = b.sub( a );
    Vec3D n = a.cross( dir.normalize( ) );
    c.addSelf( n.normalizeTo( dir.magnitude() * toxi::math::MathUtils::SQRT3 / 2 ) );
    return Triangle3D( a, b, c );
}

/*
============
idQuat::ToAngularVelocity
============
*/
Vec3D idQuat::ToAngularVelocity( void ) const {
	Vec3D vec;

	vec.x = x;
	vec.y = y;
	vec.z = z;
	vec.Normalize();
	return vec * Float_ACos( w );
}

Color Cylinder::getDifColor(const Vec3D& pnt, const CIsect& isect/* = CIsect()*/) const
{
	if (mMtrl->DifTexture)
	{
		Vec3D texCoords = getTexCoords(pnt, isect);
		return scale3D(mMtrl->DifTexture->sample(texCoords.x(), texCoords.y()), mMtrl->DifColor);
	}
	return mMtrl->DifColor;
}

toxi::geom::Vec3D toxi::geom::Matrix4x4::applyToSelf( Vec3D & v )
{
	for ( int i = 0; i < 4; i++ ) {
		double* m = matrix[ i ];
		temp[ i ] = v.getX() * m[ 0 ] + v.getY() * m[ 1 ] + v.getZ() * m[ 2 ] + m[ 3 ];
	}
	v.set( ( float ) temp[ 0 ], ( float ) temp[ 1 ], ( float ) temp[ 2 ] ).scaleSelf(
		( float ) ( 1.0 / temp[ 3 ] ) );
	return v;
}

void Matrix<float>::translate(const Vec3D& vec)
{
	const float x = vec._x();
	const float y = vec._y();
	const float z = vec._z();

	matrix[12] += matrix[0]*x + matrix[4]*y + matrix[8]*z;
	matrix[13] += matrix[1]*x + matrix[5]*y + matrix[9]*z;
	matrix[14] += matrix[2]*x + matrix[6]*y + matrix[10]*z; 
}

Vec3D CSceneData::getVertexNormal(int x, int y)const
{
	float a = getVertexHeight(x,	y);
	float b = getVertexHeight(x,	y+1);
	float c = getVertexHeight(x+1,	y);
	Vec3D vVector0(0,(b-a),1);
	Vec3D vVector1(1,(c-a),0);
	Vec3D vN = vVector0.cross(vVector1);
	return vN.normalize();
}

void Camera::RotateLeftAroundCenter(float speed)
{
    Vec3D d = m_up.CrossProduct(m_center - m_eye);

    float tam = d.Length();

    d /= tam;

    m_eye += d * speed;
}

Vec3D CTerrainData::getVertexNormal(int nCellX, int nCellY)const
{
	float a = getVertexHeight(nCellX,	nCellY);
	float b = getVertexHeight(nCellX,	nCellY+1);
	float c = getVertexHeight(nCellX+1,	nCellY);
	Vec3D vVector0(0,(b-a),1);
	Vec3D vVector1(1,(c-a),0);
	Vec3D vN = vVector0.cross(vVector1);
	return vN.normalize();
}

Mat3D VectorPairRotation(const Vec3D& from1, const Vec3D& from2, const Vec3D& to1, const Vec3D& to2) {
  Vec3D from_x = (from1.normalized()+from2.normalized()).normalized();
  Vec3D from_y = (from1.normalized()-from2.normalized()).normalized();
  Mat3D Mfrom(from_x, from_y, from_x%from_y);

  Vec3D to_x = (to1.normalized()+to2.normalized()).normalized();
  Vec3D to_y = (to1.normalized()-to2.normalized()).normalized();
  Mat3D Mto(to_x, to_y, to_x%to_y);

  return Mto * Mfrom.inverse();
}

void Camera::moveRelative(const Vec3D& direction) {
    float elevation = position_.z() - terrain_.heightMap().elevation(Vec2D(position_.x(), position_.y()));

    Vec3D d;
    d.x() = direction.x() * std::cos(rotation_.z()) - direction.y() * std::sin(rotation_.z());
    d.y() = direction.x() * std::sin(rotation_.z()) + direction.y() * std::cos(rotation_.z());

    position_ += d;

    position_.z() = terrain_.heightMap().elevation(Vec2D(position_.x(), position_.y())) + elevation;
}

void Camera::MoveLeft(float speed)
{
    Vec3D d = m_up.CrossProduct(m_center - m_eye);

    float tam = d.Length();

    d /= tam;

    m_center += d * speed;
    m_eye += d * speed;
}

void Camera::MoveForward(float speed)
{
    Vec3D d = m_center - m_eye;

    float tam = d.Length();

    d /= tam;

    m_center += d * speed;
    m_eye += d * speed;
}

void RibbonEmitter::setup(int anim, int time)
{
	Vec3D ntpos = parent->mat * pos;
	Vec3D ntup = parent->mat * (pos + Vec3D(0, 0, 1));
	ntup -= ntpos;
	ntup.normalize();
	float dlen = (ntpos - tpos).length();

	manim = anim;
	mtime = time;

	// move first segment
	RibbonSegment &first = *segs.begin();
	if (first.len > seglen) {
		// add new segment
		first.back = (tpos - ntpos).normalize();
		first.len0 = first.len;
		RibbonSegment newseg;
		newseg.pos = ntpos;
		newseg.up = ntup;
		newseg.len = dlen;
		segs.push_front(newseg);
	}
	else {
		first.up = ntup;
		first.pos = ntpos;
		first.len += dlen;
	}

	// kill stuff from the end
	float l = 0;
	bool erasemode = false;
	for (std::list<RibbonSegment>::iterator it = segs.begin(); it != segs.end();) {
		if (!erasemode) {
			l += it->len;
			if (l > length) {
				it->len = l - length;
				erasemode = true;
			}
		}
		else {
			segs.erase(it);
		}
		++it;
	}

	tpos = ntpos;
	tcolor = Vec4D(color.getValue(anim, time), opacity.getValue(anim, time));

	tabove = above.getValue(anim, time);
	tbelow = below.getValue(anim, time);
}