vec3_t projectPointOnEdge(const vec3_t& M,const vec3_t& A, const vec3_t& u)
{
  checkVector(u);
  if(u.abs2()==0) EG_BUG;
  double k = ((M-A)*u)/u.abs2();
  return A + k*u;
}

	static vec3_t RotateZ(K rot_sin, K rot_cos, const vec3_t& v) {
		return vec3_t {
			rot_cos*v.x() - rot_sin*v.y(),
			rot_sin*v.x() + rot_cos*v.y(),
			v.z()
		};
	}

mat3_t vecs2rot(const vec3_t &a_B, const vec3_t &g) {
  // Create Quaternion from two vectors
  const double cos_theta = a_B.normalized().transpose() * g.normalized();
  const double half_cos = sqrt(0.5 * (1.0 + cos_theta));
  const double half_sin = sqrt(0.5 * (1.0 - cos_theta));
  const vec3_t w = a_B.cross(g).normalized();

  const double qw = half_cos;
  const double qx = half_sin * w(0);
  const double qy = half_sin * w(1);
  const double qz = half_sin * w(2);

  // Convert Quaternion to rotation matrix
  const double qx2 = qx * qx;
  const double qy2 = qy * qy;
  const double qz2 = qz * qz;
  const double qw2 = qw * qw;

  const double R11 = qw2 + qx2 - qy2 - qz2;
  const double R12 = 2 * (qx * qy - qw * qz);
  const double R13 = 2 * (qx * qz + qw * qy);

  const double R21 = 2 * (qx * qy + qw * qz);
  const double R22 = qw2 - qx2 + qy2 - qz2;
  const double R23 = 2 * (qy * qz - qw * qx);

  const double R31 = 2 * (qx * qz - qw * qy);
  const double R32 = 2 * (qy * qz + qw * qx);
  const double R33 = qw2 - qx2 - qy2 + qz2;

  mat3_t R;
  R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
  return R;
}

bool vec3_t::RayCylinderIntersect(const vec3_t& pStart, const vec3_t& pDir,
                                  const vec3_t& edgeStart, const vec3_t& edgeEnd,
                                  const float radius,
                                  float* f)
{
    // math. for 3d game programming 2nd ed. page 270
    // assert(edgeStart != edgeEnd); // sloppy level design
    const vec3_t pa = edgeEnd - edgeStart;
    const vec3_t s0 = pStart - edgeStart;
    const float pa_squared = pa.AbsSquared();
    const float pa_isquared = 1.0f/pa_squared;

    // a
    const float pva = pDir * pa;
    const float a = pDir.AbsSquared() - pva*pva*pa_isquared;

    // b
    const float b = s0*pDir - (s0*pa)*(pva)*pa_isquared;

    // c
    const float ps0a = s0*pa;
    const float c = s0.AbsSquared() - radius*radius - ps0a*ps0a*pa_isquared;

    const float dis = b*b - a*c;
    if(dis < 0)
        return false;

    *f = (-b - lynxmath::Sqrt(dis))/a;
    const float collision = (pStart + *f*pDir - edgeStart)*pa;
    return collision >= 0 && collision <= pa_squared;
}

vec2_t projectVectorOnPlane(vec3_t V,vec3_t i,vec3_t j)
{
  if(i.abs2()==0) EG_BUG;
  if(j.abs2()==0) EG_BUG;
  double x = V*i/i.abs2();
  double y = V*j/j.abs2();
  return vec2_t(x,y);
}

void CHudHealth::CalcDamageDirection(vec3_t vecFrom)
{
	vec3_t	forward, right, up;
	float	side, front;
	vec3_t vecOrigin, vecAngles;

	if (!vecFrom[0] && !vecFrom[1] && !vecFrom[2])
	{
		m_fAttackFront = m_fAttackRear = m_fAttackRight = m_fAttackLeft = 0;
		return;
	}


	memcpy(vecOrigin, gHUD.m_vecOrigin, sizeof(vec3_t));
	memcpy(vecAngles, gHUD.m_vecAngles, sizeof(vec3_t));


	VectorSubtract (vecFrom, vecOrigin, vecFrom);

	float flDistToTarget = vecFrom.Length();

	vecFrom = vecFrom.Normalize();
	AngleVectors (vecAngles, forward, right, up);

	front = DotProduct (vecFrom, right);
	side = DotProduct (vecFrom, forward);

	if (flDistToTarget <= 50)
	{
		m_fAttackFront = m_fAttackRear = m_fAttackRight = m_fAttackLeft = 1;
	}
	else 
	{
		if (side > 0)
		{
			if (side > 0.3)
				m_fAttackFront = max(m_fAttackFront, side);
		}
		else
		{
			float f = fabs(side);
			if (f > 0.3)
				m_fAttackRear = max(m_fAttackRear, f);
		}

		if (front > 0)
		{
			if (front > 0.3)
				m_fAttackRight = max(m_fAttackRight, front);
		}
		else
		{
			float f = fabs(front);
			if (f > 0.3)
				m_fAttackLeft = max(m_fAttackLeft, f);
		}
	}
}

void Wavefront::Transform(vec3_t& v, const LLXform* x) //recursive
{
	LLMatrix4 m;
	x->getLocalMat4(m);
	for (vec3_t::iterator iterv = v.begin(); iterv != v.end(); ++iterv)
	{
		*iterv = *iterv * m;
	}

	if (const LLXform* xp = x->getParent()) Transform(v, xp);
}

float vec3_t::GetAngleDeg(const vec3_t& a, const vec3_t& b)
{
    float d = a.Abs() * b.Abs();
#ifdef _DEBUG
    if(fabsf(d) < lynxmath::EPSILON)
    {
        assert(0);
        return 0.0f;
    }
#endif
    return acosf((a * b) / d) * lynxmath::RADTODEG;
}

double angle(const vec3_t & u, const vec3_t & v)
{
  // return the angle w.r.t. another 3-vector
  double ptot2 = u.abs2()*v.abs2();
  if(ptot2 <= 0) {
      return 0.0;
  } else {
    double arg = (u*v)/sqrt(ptot2);
    if(arg >  1.0) arg =  1.0;
    if(arg < -1.0) arg = -1.0;
    return acos(arg);
  }
}

vec3_t CPartEffects::RandomRoundVec( vec3_t vMin, vec3_t vMax )
{
	float flMin, flMax;

	flMin = vMin.Length( );
	flMax = vMax.Length( );

	vec3_t vVec( gEngfuncs.pfnRandomFloat( vMin.x, vMax.x ), gEngfuncs.pfnRandomFloat( vMin.y, vMax.y ),
		gEngfuncs.pfnRandomFloat( vMin.z, vMax.z ) );
	vVec = vVec.Normalize( ) * gEngfuncs.pfnRandomFloat( flMin, flMax );

	return vVec;
}

void CParticleSystem::Render(const vec3_t& side, const vec3_t& up, const vec3_t& dir)
{
    glBindTexture(GL_TEXTURE_2D, m_texture);
    glBegin(GL_QUADS);

    // use vbos instead of this
    std::vector<particle_t>::const_iterator iter;
    for(iter = m_particles.begin();iter != m_particles.end(); ++iter)
    {
        if(iter->lifetime < 0.0f)
            continue;

        const vec3_t X = iter->size*0.5f*side;
        const vec3_t Y = iter->size*0.5f*up;
        const vec3_t Q1 =  X + Y + iter->origin;
        const vec3_t Q2 = -X + Y + iter->origin;
        const vec3_t Q3 = -X - Y + iter->origin;
        const vec3_t Q4 =  X - Y + iter->origin;

        glColor4f(iter->color.x, iter->color.y, iter->color.z, iter->alpha);

        glTexCoord2f(1.0f, 1.0f);
        glVertex3fv(Q1.GetPointer());
        glTexCoord2f(0.0f, 1.0f);
        glVertex3fv(Q2.GetPointer());
        glTexCoord2f(0.0f, 0.0f);
        glVertex3fv(Q3.GetPointer());
        glTexCoord2f(1.0f, 0.0f);
        glVertex3fv(Q4.GetPointer());

    }

    glEnd();
}

void V_GetDeathCam(cl_entity_t * ent1, cl_entity_t * ent2, float * angle, float * origin)
{
	float newAngle[3];
	float newOrigin[3];
	static vec3_t nonDestructedOrigin;
	float distance = 168.0f;

	v_lastDistance += v_frametime * 96.0f;	// move unit per seconds back

	if ( v_resetCamera )
		v_lastDistance = 64.0f;

	if ( distance > v_lastDistance )
		distance = v_lastDistance;

	if (ent1->origin.x == 0 && ent1->origin.y == 0 && ent1->origin.z == 0)
		nonDestructedOrigin.CopyToArray(newOrigin);
	else
	{
		nonDestructedOrigin = ent1->origin;
		VectorCopy(ent1->origin, newOrigin);
	}

	if ( ent1->player )
		newOrigin[2]+= 17; // head level of living player

	// get new angle towards second target
	if ( ent2 )
	{
		VectorSubtract(ent2->origin, nonDestructedOrigin, newAngle);
		VectorAngles( newAngle, newAngle );
		newAngle[0] = -newAngle[0];
	}
	else
	{
		// if no second target is given, look down to dead player
		newAngle[0] = 90.0f;
		newAngle[1] = 0.0f;
		newAngle[2] = 0;
	}

	// and smooth view
	V_SmoothInterpolateAngles( v_lastAngles, newAngle, angle, 120.0f );

	V_GetChaseOrigin( angle, newOrigin, distance, origin );

	VectorCopy(angle, v_lastAngles);
}

bool LaplaceSmoother::setNewPosition(vtkIdType id_node, vec3_t x_new)
{
  using namespace GeometryTools;

  vec3_t x_old;
  m_Grid->GetPoint(id_node, x_old.data());
  EG_VTKDCN(vtkCharArray, node_type, m_Grid, "node_type");
  bool move = true;
  if(m_NoCheck) {
    return move;
  }
  QVector<vec3_t> old_cell_normals(m_Part.n2cGSize(id_node));
  EG_VTKDCC(vtkIntArray, cell_code, m_Grid, "cell_code");
  for (int i = 0; i < m_Part.n2cGSize(id_node); ++i) {
    old_cell_normals[i] = GeometryTools::cellNormal(m_Grid, m_Part.n2cGG(id_node, i));
  }
  m_Grid->GetPoints()->SetPoint(id_node, x_new.data());

  for (int i = 0; i < m_Part.n2cGSize(id_node); ++i) {
    vec3_t n = GeometryTools::cellNormal(m_Grid, m_Part.n2cGG(id_node, i));
    if (n*old_cell_normals[i] < 0.2*old_cell_normals[i].abs2()) {
      move = false;
      break;
    }
  }
  if (!move) {
    m_Grid->GetPoints()->SetPoint(id_node, x_old.data());
  }
  return move;
}

BrlCadInterface::HitType BrlCadInterface::shootRay(vec3_t x, vec3_t v, vec3_t &x_hit, vec3_t &n_hit, double &r)
{
  HitType hit_type = Miss;
  v.normalise();
  vec3_t x_in, x_out, n_in, n_out;
  double r_in, r_out;
  if (brlCadShootRay(x, v, x_in, x_out, n_in, n_out, r_in, r_out)) {
    double d_in = (x_in - x)*v;
    if (d_in > 0) {
      x_hit = x_in;
      n_hit = n_in;
      r = r_in;
      hit_type = HitIn;
    }
    double d_out = (x_out - x)*v;
    if (d_out > 0) {
      if (hit_type == Miss || d_out < d_in) {
        x_hit = x_out;
        n_hit = n_out;
        hit_type = HitOut;
        r = r_out;
      }
    }
  }
  return hit_type;
}

void CMixer::Update(const float dt, const uint32_t ticks)
{
    OBJITER iter;
    CObj* obj;
    bool success;

    for(iter=m_world->ObjBegin();iter!=m_world->ObjEnd();iter++)
    {
        obj = (*iter).second;

        if(obj->GetSound() && !obj->GetSoundState()->is_playing)
        {
            success = obj->GetSound()->Play(obj->GetSoundState());

            CObj* localplayer = m_world->GetLocalObj();
            if(success && localplayer)
            {
                // Distance to sound source
                const vec3_t diff = localplayer->GetOrigin() - obj->GetOrigin();
                const float dist = std::min(diff.Abs(), SOUND_MAX_DIST);
                int volume = (int)(dist*255/SOUND_MAX_DIST);
                if(volume > 255)
                    volume = 255;

                uint16_t angle; // 0-360 deg. for Mix_SetPosition
                vec3_t playerlook; // player is looking in this direction
                float fAlpha; // riwi to sound source
                float fBeta; // riwi look dir
                m_world->GetLocalController()->GetDir(&playerlook, NULL, NULL);

                fAlpha = atan2(diff.x, -diff.z);
                fBeta = atan2(playerlook.x, -playerlook.z);
                angle = (uint16_t)((fAlpha - fBeta)*180/lynxmath::PI);
                Mix_SetPosition(obj->GetSoundState()->cur_channel,
                                angle, (uint8_t)volume);
            }
        }
    }
}

vec3_t rotate(vec3_t v, vec3_t axis, double theta)
{
  axis.normalise();
  
  // transposed base of rotate system
  mat3_t g_t;

  // compute projection of v in axis direction
  vec3_t v_axis = (axis*v)*axis;

  // compute first orthogonal vector (first base vector)
  g_t[0] = v-v_axis;
  
  //In case of points on the rotation axis, do nothing
  if(g_t[0].abs()==0) return v;
  
  g_t[0].normalise();

  // second base vector is the normalised axis
  g_t[1] = axis;

  // compute second orthogonal vector (third base vector)
  g_t[2] = g_t[0].cross(g_t[1]);

  // base of rotate system
  mat3_t g = g_t.transp();

  // matrix for rotation around g_t[1];
  mat3_t rot = mat3_t::identity();
  rot[0][0] =  cos(theta);
  rot[0][2] =  sin(theta);
  rot[2][0] = -sin(theta);
  rot[2][2] =  cos(theta);

  // transfer v to rotate system
  vec3_t v_r = g_t*v;

  // rotate the vector and transfer it back
  v_r = rot*v_r;
  v = g*v_r;

  return v;
}

void cart2spherical(vec3_t x, double &alpha, double &beta, double &r)
{
  r = x.abs();
  static const vec3_t ex(1,0,0);
  vec3_t xy(x[0],x[1],0);
  if (x[1] >= 0) {
    alpha = angle(ex, xy);
  } else {
    alpha = 2*M_PI - angle(xy, ex);
  }
  if (xy.abs2() > 0) {
    if (x[2] >= 0) {
      beta = angle(xy, x);
    } else {
      beta = -angle(xy, x);
    }
  } else {
    beta = 0.5*M_PI;
  }
}

bool CreateCadTesselation::shootRay(vec3_t x, vec3_t v, vec3_t &x_in, vec3_t &x_out, vec3_t &n_in, vec3_t &n_out)
{
  v.normalise();
  double r_hit;
  vec3_t x_hit, n_hit;
  CadInterface::HitType hit_type = m_CadInterface->shootRay(x, v, x_hit, n_hit, r_hit);
  if (hit_type == CadInterface::Miss || hit_type == CadInterface::HitOut) {
    return false;
  }
  x_in = x_hit;
  n_in = n_hit;
  x = x_in;
  do {
    x += 1e-10*v;
    hit_type = m_CadInterface->shootRay(x, v, x_hit, n_hit, r_hit);
    if (hit_type == CadInterface::HitOut) {
      x_out = x_hit;
      n_out = n_hit;
    }
    x = x_hit;
  } while (hit_type == CadInterface::HitIn);
  return true;
}

vec3_t intersectionOnPlane(vec3_t v, vec3_t A, vec3_t nA, vec3_t B, vec3_t nB)
{
  vec3_t u = B-A;
//   u.normalise();
  v.normalise();
  v = u.abs()*v;
  
  //cout<<"u="<<u<<" v="<<v<<endl;
  
  vec2_t p_A(0,0);
  vec2_t p_B(1,0);
  vec2_t p_nA = projectVectorOnPlane(nA,u,v);
  vec2_t p_nB = projectVectorOnPlane(nB,u,v);
  
  vec2_t p_tA = turnRight(p_nA);
  vec2_t p_tB = turnRight(p_nB);
  
  double k1, k2;
  vec2_t p_K;
  if(!intersection(k1, k2, p_A, p_tA, p_B, p_tB)) {
    //qDebug()<<"WARNING: No intersection found!!!";
    p_K = 0.5*(p_A + p_B);
  }
  else {
    p_K = p_A + k1*p_tA;
  }
  
  //cout<<"nA="<<nA<<endl;
  //cout<<"p_nA="<<p_nA<<endl;
  //cout<<"p_tA="<<p_tA<<endl;
  //cout<<"p_K="<<p_K<<endl;
  if(p_K[0]<0) p_K[0] = 0;
  if(p_K[0]>1) p_K[0] = 1;
  vec3_t K = A + p_K[0]*u + p_K[1]*v;
  //cout<<"K="<<K<<endl;
  return K;
}

vec3_t projectPointOnPlane(const vec3_t& M, const vec3_t& A, const vec3_t& N)
{
  double k = ((M-A)*N)/N.abs2();
  return( M - k*N );
}