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 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 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);
}
vec2f operator-(const vec2f& v1, const vec2f &v2){
return vec2f(v1.peekx()-v2.peekx(),
v1.peeky()-v2.peeky());
}
void nkDebug::resetCursor()
{
// 320, 480
g_cursor.SetXY((-320.0f/2.0)+10.0, (480.0/2.0)-10.0f-6.0f);
}