toxi::geom::Vec3D toxi::geom::Vec2D::bisect( Vec2D b )
{
	Vec2D diff = this->sub(b);
	Vec2D sum = this->add(b);
	float dot = diff.dot(sum);
	return Vec3D(diff.x, diff.y, -dot / 2)
}

void addVertices(vector<float>& vertices, vector<float>& colors,
                 Vec2D<float> point, Vec2D<float> v1, QColor color,
                 float z = 0.0) {
  Vec2D<float> normal = v1;
  swap(normal.x, normal.y);
  normal.x *= -1;
  normal.normalize();

  Vec2D<float> p1 = point + normal * LINE_WIDTH;
  Vec2D<float> p2 = point - normal * LINE_WIDTH;

  for (int i = 0; i < 2; i++) {
    colors.push_back(color.redF());
    colors.push_back(color.greenF());
    colors.push_back(color.blueF());
    colors.push_back( (color.greenF() + color.redF()) *0.3f  );
  }

  vertices.push_back(p1.x);
  vertices.push_back(p1.y);
  vertices.push_back(z);

  vertices.push_back(p2.x);
  vertices.push_back(p2.y);
  vertices.push_back(z);
}

toxi::geom::Polygon2D * toxi::geom::Polygon2D::smooth( const float & amount, const float & baseWeight )
{
	Vec2D centroid = getCentroid();
	int num = vertices.size();
	std::vector<Vec2D> filtered;
	for (int i = 0, j = num - 1, k = 1; i < num; i++) {
		Vec2D a = vertices.at(i);
		Vec2D dir = vertices.at(j).sub( a ).addSelf( vertices.at(k).sub( a ))
			.addSelf(a.sub( centroid).scaleSelf(baseWeight));
		filtered.push_back(a.add(dir.scaleSelf(amount)));
		j++;
		if (j == num) {
			j = 0;
		}
		k++;
		if (k == num) {
			k = 0;
		}
	}
	vertices.clear();

	for( auto it = filtered.begin(); it != filtered.end(); ++it ) 
	{
		vertices.push_back( *it );
	}

	return this;
}

static SimpleString StringFrom(const Vec2D & v)
{
    SimpleString s = SimpleString();
    s += SimpleString("(");
    s += StringFrom(v.get_x());
    s += SimpleString(",");
    s += StringFrom(v.get_y());
    s += SimpleString(")");

    return s;
}

	void updateCenterOfMass(Body body) {
		if (_mass == 0) {
			_mass = body.getMass();
			_centerOfMass = body.getPos();
		} else {
			double new_mass = _mass + body.getMass();
			double new_cen_x = ((_mass*_centerOfMass.getX()) + (body.getMass() * body.getXPos()))/new_mass;
			double new_cen_y = ((_mass*_centerOfMass.getY()) + (body.getMass() * body.getYPos()))/new_mass;
			_centerOfMass = Vec2D(new_cen_x, new_cen_y);
			_mass = new_mass;
		}
	}

toxi::geom::Polygon2D::Polygon2D( const Vec2D & baseA, const Vec2D & baseB, const int & res )
{
	double theta = -(toxi::math::MathUtils::PI - (toxi::math::MathUtils::PI * (res - 2) / res));
	Vec2D dir = baseB.sub( baseA);
	Vec2D prev = baseB;
	add( baseA );
	add( baseB );
	for (int i = 1; i < res - 1; i++) {
		Vec2D p = prev.add(dir.getRotated(theta * i));
		add(p);
		prev = p;
	}
}

	void SimpleGen::generate( Weapon* weapon )
	{
		Object* owner = weapon->owner;
		Object* destObj = weapon->destObj;

		Vec2D dif = destObj->getPos() - owner->getPos();
		float s= sqrtf(dif.length2());
		Vec2D speed = ( weapon->bulletSpeed / s ) * dif;

		Object* bullet = weapon->createBullet();
		bullet->setVelocity( speed );
		spawnObject(bullet);
	}

	Force calculateForceFromBody(Body otherBody) {
		Vec2D distance = calculateDistanceFromBody(otherBody);
		Vec2D unit_distance = distance.getUnitVector();
		if (distance.getMagnitude() != 0) {
			double forceMagnitude= (GRAVITYCONST * otherBody.getMass() * getMass()) / (pow(distance.getMagnitude(), 2));

			double forceMagnitude_x = forceMagnitude * unit_distance.getX();
			double forceMagnitude_y = forceMagnitude * unit_distance.getY();
			return Force(forceMagnitude_x, forceMagnitude_y);
		} else {
			return Force(0,0);
		}
	}

void SchQ_Particle_System :: apply_hooks_law(Spring &s){
    Vec2D<double> pos0  = s.a->get_position(), pos1  = s.b->get_position(),
                  vel0  = s.a->get_velocity(), vel1  = s.b->get_velocity();
    double        dx    = pos0.x-pos1.x,       dy    = pos0.y-pos1.y,
                  dvdx  = vel0.x-vel1.x,       dvdy  = vel0.y-vel1.y,
                  dist  = pos0.distance(pos1), rdist  = dist != 0 ? 1.0/dist : 0;
    
    Vec2D<double> force  (-(s.stiffness*(dist-s.rest_length) +
                            s.dampening*(dvdx) * dx*rdist) * dx*rdist ,
                          -(s.stiffness*(dist-s.rest_length) +
                            s.dampening*(dvdy) * dy*rdist) * dy*rdist);
    
    s.a->add_force(force);
    s.b->add_force(force.multiply(-1));
}

	DirArrayGen::DirArrayGen( Vec2D const& d )
	{
		difPos = Vec2D(0,0);
		dir = ( 1.0f / sqrtf( d.length2() ) ) * d;
		num = 1;
		offset = 2;
	}

toxi::geom::Vec2D toxi::geom::Polygon2D::getClosestPointTo( Vec2D & p )
{
	float minD = FLT_MAX;
	Vec2D q = Vec2D();
	for (Line2D l : getEdges()) 
	{
		Vec2D c = l.closestPointTo(p);
		float d = static_cast< float > ( c.distanceToSquared( p ) );
		if (d < minD) 
		{
			q = c;
			minD = d;
		}
	}
	return q;
}

Vec2D Segment::intersectionPoint(Segment const& other) const
{
  Vec2D selfDelta = delta();
  Vec2D otherDelta = other.delta();

  if(selfDelta.cross(otherDelta) != 0)
  {
    float t = other.a.subtract(a).cross(otherDelta) / selfDelta.cross(otherDelta);
    float u = other.a.subtract(a).cross(selfDelta) / selfDelta.cross(otherDelta);
    if(t >= 0 && t <= 1 && u >= 0 && u <= 1)
    {
      return a.add(selfDelta.scale(t));
    }
  }

  return Vec2D(0, 0);
}

Image BrushStrokerCustomBrush::drawDabImage(const TabletInputData &data, QRect *rect)
{
    Q_ASSERT(data.pressure > 0);

    Vec2D radiusVec;
    radiusVec.rx() = radiusBase() * data.pressure;
    radiusVec.ry() = radiusVec.x() * (1.0 - _setting.flattening);

    _lastMinorRadius = radiusVec.y();

    //qDebug() << "radius" << radiusVec.x << radiusVec.y;

    QPainterPath ellipse;
    ellipse.addEllipse(data.pos, radiusVec.x(), radiusVec.y());

    QRect dabRect;

    if (_setting.rotation)
    {
        QTransform rotation;
        rotation.rotate(_setting.rotation);
        ellipse = rotation.map(ellipse);

        dabRect = ellipse.boundingRect().toAlignedRect();
    }
    else
    {
        dabRect = QRectF(QPointF(data.pos - radiusVec), QSizeF(2.0 * radiusVec)).toAlignedRect();
    }

    Image dabImage(dabRect.size());
    dabImage.clear();

    Painter dabPainter(&dabImage);

    dabPainter.translateShape(-dabRect.topLeft());

    if (_setting.tableWidth == 1 && _setting.tableHeight == 1)
    {
        // no gradient
        dabPainter.setPixel(pixel());
    }
    else
    {
        ArgbGradient gradient;
        gradient.addStop(0, pixel());
        gradient.addStop(_setting.tableWidth, pixel() * _setting.tableHeight);
        gradient.addStop(1, Pixel(0));

        dabPainter.setBrush(Malachite::Brush::fromRadialGradient(gradient, data.pos, radiusVec));
    }

    dabPainter.drawPath(ellipse);

    dabPainter.end();

    *rect = dabRect;
    return dabImage;
}

bool circleLineIntersect(Vec2D const& p, float const r, Vec2D const& l1, Vec2D const& l2, float const lr)
{
  Vec2D ld = l2 - l1;
  Vec2D ln = ld.normal();
  Vec2D l1p = p - l1;
  Vec2D l2p = p - l2;
  if(ln.cross(l1p) * ln.cross(l2p) < 0)
  {
    return l1p.projectioni(ln).lengthSquared() <= (r + lr) * (r + lr);
  }
  else if(l1p.lengthSquared() < l2p.lengthSquared())
  {
    return l1p.lengthSquared() <= (r + lr) * (r + lr);
  }
  else
  {
    return l2p.lengthSquared() <= (r + lr) * (r + lr);
  }
}

float toxi::geom::Vec2D::angleBetween( Vec2D v, bool forceNormalize )
{
	double theta;
	if (forceNormalize) {
		theta = getNormalized().dot(v.getNormalized());
	} else {
		theta = dot(v);
	}
	return (float) std::acos(toxi::math::MathUtils::clipNormalized(theta));
}

toxi::geom::Vec2D toxi::geom::Polygon2D::getRandomPoint()
{
	std::vector< Line2D > edges = getEdges();
	int numEdges = edges.size();
	Line2D ea = edges.at( static_cast< int > ( toxi::math::MathUtils::random( numEdges ) ) );
	Line2D * eb = nullptr;
	// and another one, making sure it's different
	while (eb == nullptr || *eb == ea) 
	{
		eb = &edges.at( static_cast< int > ( toxi::math::MathUtils::random( numEdges ) ) );
	}
	// pick a random point on edge A
	Vec2D p = ea.getA().interpolateTo(ea.getB(), toxi::math::MathUtils::random( 1.0 ) );
	// then randomly interpolate to another random point on edge B
	Vec2D ret = p.interpolateToSelf(
		eb->getA().interpolateTo(eb->getB(), toxi::math::MathUtils::random( 1.0 ) ),
		static_cast< float > ( toxi::math::MathUtils::random( 1.0 ) ) );
	delete eb;
	return ret;
}

int toxi::geom::Vec2D::compareTo( Vec2D v )
{
	if (x == v.x && y == v.y) {
		return 0;
	}
	float a = magSquared();
	float b = v.magSquared();
	if (a < b) {
		return -1;
	}
	return +1;
}

Debris::Debris(Ship* s, Player* p, Vec2D location, Vec2D direction, bool collides, bool draw) : ParticleEffect(0, p, none, location, direction, NULL, collides, draw)
{
	this->mesh = this;
	shipFinalDir = s->dir;
	Vec2D rotationpoint;
	Vec2D midpoint;
	Mesh* m = (Mesh*)(s->mesh);
	Mesh* meshpart = NULL;
	for(unsigned int i = 0; i < m->polyNum; ++i)
	{
		for(unsigned int p = 0; p < m->polys[i].length - 1; ++p)
		{
			meshpart = new Mesh();
			meshpart->polyNum = 1;
			meshpart->mountNum = 0;
			meshpart->polys = new sf2::Polygon[1];
			meshpart->polys[0].length = 2;
			meshpart->polys[0].vertices = new sf2::CUSTOMVERTEX[2];
			meshpart->polys[0].vertices[0] = m->polys[i].vertices[p];
			meshpart->polys[0].vertices[1] = m->polys[i].vertices[p+1];
			midpoint = Vec2D(meshpart->polys[0].vertices[1].X + meshpart->polys[0].vertices[0].X, meshpart->polys[0].vertices[1].Y + meshpart->polys[0].vertices[0].Y)/2.0f;
			meshpart->radius = Distance(Vec2D(meshpart->polys[0].vertices[0].X, meshpart->polys[0].vertices[0].Y), Vec2D(meshpart->polys[0].vertices[1].X, meshpart->polys[0].vertices[1].Y)) / 2.0f;
			Vec2D addedVel = 3.0f/midpoint;	//difference between midpoint and ship's midpoint
			addedVel.scale(meshpart->radius);	//scale by size of part

			//rotationpoint
			Vec2D start(m->polys[i].vertices[p]);
			Vec2D end = Vec2D(m->polys[i].vertices[p+1]);
			rotationpoint = start - end;
			rotationpoint.scale(randomFloat(0.0f, 1.0f));
			rotationpoint += end;

			float limit = 3.14159f * 2 / meshpart->radius;

			float rotationspeed = randomFloat(-limit, limit);
			LoadToVRAM(meshpart);
			parts.insert(parts.end(), new DebrisPart(meshpart, randomFloat(1.0f, 5.0f), s->loc, s->vel + addedVel, rotationpoint, midpoint, rotationspeed));
		}
	}
}

WheelsControl::WheelOutput DriveEquation::computeWheel(const WheelConfig &wconfig, const Motion &motion) const {
	Vec2D relvel = motion.vel.rotate(motion.curdir);

	Vec2D wheelout = relvel + (wconfig.framepos*motion.angvel).rotate(-M_PI/2);
	float mag = wheelout.magnitude();

	WheelsControl::WheelOutput out;
	if (mag < config.minspeed) {
		out.angle = 0;
		out.effort = 0;
		out.enabled = false;
		return out;
	}

	out.angle = (wheelout.angle()+config.wheelangleoffset).getRad();
	out.effort = wconfig.effortscale*mag;
	if (out.effort > 0)
		out.effort += wconfig.effortoffset;
	else
		out.effort -= wconfig.effortoffset;
	return out;
}

float Vec2D::rotationTo(Vec2D const& other) const
{
  float myAngle = angle();
  float otherAngle = other.angle();
  if(myAngle > otherAngle)
  {
    return myAngle - (otherAngle + 1.0);
  }
  else
  {
    return myAngle - otherAngle;
  }
}