void generateInitialTraj(MotionOptimizer & chomper,
                         int N, 
                         const vec2f& p0, 
                         const vec2f& p1,
                         MatX& q0,
                         MatX& q1) {
  
    q0.resize(1, 2);
    q1.resize(1, 2);

    q0 << p0.x(), p0.y();
    q1 << p1.x(), p1.y();

    chomper.getTrajectory().initialize( q0, q1, N );
  
}

bool baryCentricTriangle(vec2f p, vec3f v1, vec3f v2, vec3f v3, float &u, float &v, float &r)
{
	float x1mx3 = v1.x() - v3.x();
	float x2mx3 = v2.x() - v3.x();
	float y1my3 = v1.y() - v3.y();
	float y2my3 = v2.y() - v3.y();

	float det = (x1mx3 * y2my3) - (y1my3*x2mx3);

	float pxmx3 = p.x() - v3.x();
	float pymy3 = p.y() - v3.y();

	if(det == 0.0 || det == -0.0) return false;

	u = (y2my3*pxmx3 + x2mx3*-1*pymy3)/det;
	v = (y1my3*-1*pxmx3 + x1mx3*pymy3)/det;
	r = 1-u-v;

	if (u > 1.0f || v > 1.0f || r > 1.0f) return false;
	if (u < 0.0f || v < 0.0f || r < 0.0f) return false;

	return true;
}

 void BacteroidsState::PlayerCollision(Player *me, GameObject *obj, const vec2f &objToMe, float dist)
 {
     if (obj->GetType() == Bacter::TYPE)
     {
         vec2f v = objToMe.SafeNormalized() * dist/8.0f;
         vec2f p = me->GetPosition();
         me->AddVelocity(v);
         me->SetPosition(p + (v / 2.0f));
         me->TakeHit(m_soundPlayer);
         m_damageFade.Activate(1.0f);
         if (me->IsDead())
         {
             m_damageFade.SetFadeAcceleration(0.0f);
             m_damageFade.Activate(1.0f);
             m_soundPlayer.PlayPlayerDeathSound(p.x, p.y);
         }
     }
 }

 void BacteroidsState::BacterCollision(Bacter *me, GameObject *obj, const vec2f &objToMe, float dist)
 {
     if (obj->GetType() == Bacter::TYPE || obj->GetType() == Player::TYPE)
     {
         vec2f v = objToMe.SafeNormalized() * dist/8.0f;
         vec2f p = me->GetPosition();
         me->AddVelocity(v);
         me->SetPosition(p + (v / 2.0f));
         float r = me->GetRadius() + obj->GetRadius();
         me->ModifySize(1.0f + dist / r);
     }
     else if (obj->GetType() == Projectile::TYPE)
     {
         if (me->CanSplit())
         {
             m_score += me->GetPoints();
             m_kills++;
             me->AddVelocity(obj->GetVelocity() * 0.5f);
             SplitBacter(me);
         }
     }
 }

 void Shader::setUniform(const std::string &name, const vec2f v, bool warn) {
     glUniform2f(uniform(name, warn), v.x(), v.y());
 }

 void Camera::setViewport(vec2f dim)
 {
     vpWidth = dim.x();
     vpHeight = dim.y();
     hasProjChanged = true;
 }

void MoverVehicle::directionControl(const vec2f &dir, float speed)
{
	float t = 0.5;
	if(speed == 0.f)
		t = 1.0;
	Pose::vec unitDirection = getMaster()->getDirection();
	float forward = 0, angle = 0;
	// 1. Interpolate control
	Pose::vec control = dir * t + (1.f - t) * lastControl;
	lastControl = control;
	// 2. Calculate drive control for kinematics mode
	if(definition->kinematic)
	{
		//float errorAngle = dir.length_squared() > 0.f ? vecAngle2d_CW(dir,unitDirection) : 0.f;//,vecAngle2d_CW(sumForce,unit->getDirection())};
		float error[] = 
		{
			(speed - currentVelocity[0]) * fSign(unitDirection & control),
			dir.length_squared() > 0.f ? vecAngle2d_CW(control,unitDirection) : 0.f,
		};
	//	// 4. calculate prefered direction
		if(error[1] > M_PI)
			error[1] -= (2 * M_PI);
		float turnTime = fabs(error[1] / currentVelocity[1]);
		float brakingTime = sqrt(2 * fabs(error[1]) / definition->acceleration[1]);
		if( currentVelocity[1] * error[1] >= 0.0)
		{
			
			if(turnTime < brakingTime)
				angle = -error[1];
			else
				angle = fSign(error[1]);

			//if(fabs(turnTime) < 0.1)
			//	angle = 0;
		}
		else
		{
			angle = (error[1] >= 0) ? 1.f : -1.f;
		}
		//angle = fSign(errorAngle);
		//angle = errorAngle;
		//angle = -fSign(currentVelocity[1]);
		forward = clamp(deadZone(error[0],-0.1f,0.1f),-1.f,1.f);
		//angle = fSign(deadZone(angle,-0.1f,0.1f));
		angle = clamp(angle,-1.f,1.f);
	}
	else
	{
		// 4. calculate prefered direction
		forward = speed;//(dir & unitDirection);
		angle = control.length_squared()>0.f?vecAngle2d_CW(control,unitDirection):0.f;//,vecAngle2d_CW(sumForce,unit->getDirection())};

		if(angle > M_PI)
			angle -= (M_PI+M_PI);

		forward = clamp(deadZone(speed,-0.1f,0.1f),-1.f,1.f);
		angle = fSign(deadZone(angle,-0.1f,0.1f));
	}
	// 4. fix values
	if(fabs(forward)>1.f)
		forward = 0.f;
	if(fabs(angle)>1.f)
		angle = 0.f;	
	// 5. apply control
	query_Direction(portLinear,dcmdDir_set, forward);	
	query_Direction(portAngular,dcmdDir_set, angle);
}

float dot(const vec2f& v1, const vec2f& v2){
    return ((v1.peekx()*v2.peekx())+
            (v1.peeky()*v2.peeky()));
}

vec2f operator*(const float k, const vec2f& v1){
    return vec2f(v1.peekx()*k,
                 v1.peeky()*k);
}

void drawChar(const char c, vtxdf *verticies)
{
  u32 gx, gy;
  getGlyphCoords(c, &gx, &gy);

  f32 s = 6.0f / 64.0f;
  f32 u = gx * s;
  f32 v = gy * s;

  vtxdf *bottom_left  = &verticies[0];
  vtxdf *bottom_right = &verticies[1];
  vtxdf *top_left     = &verticies[2];
  vtxdf *top_right    = &verticies[5];

  f32 xMin = g_cursor._x;
  f32 yMin = g_cursor._y;
  f32 xMax = xMin + 6.0f;
  f32 yMax = yMin + 6.0f;

  bottom_left ->position.SetXY(xMin, yMin);
  bottom_right->position.SetXY(xMax, yMin);
  top_left    ->position.SetXY(xMin, yMax);
  top_right   ->position.SetXY(xMax, yMax);

  f32 uMin = u;
  f32 vMin = v;
  f32 uMax = u + s;
  f32 vMax = v + s;

  bottom_left ->uv.SetXY(uMin, vMax);
  bottom_right->uv.SetXY(uMax, vMax);
  top_left    ->uv.SetXY(uMin, vMin);
  top_right   ->uv.SetXY(uMax, vMin);

  verticies[3] = *top_left;
  verticies[4] = *bottom_right;

  g_cursor.SetX(xMax);
}

void nkDebug::resetCursor()
{
  // 320, 480
  g_cursor.SetXY((-320.0f/2.0)+10.0, (480.0/2.0)-10.0f-6.0f);
}