toxi::geom::Vec2D toxi::geom::Polygon2D::getCentroid()
{
Vec2D res = Vec2D();
for (int i = 0, num = vertices.size(); i < num; i++)
{
Vec2D a = vertices.at(i);
Vec2D b = vertices.at( ( i + 1 ) % num );
double crossP = a.getX() * b.getY() - b.getX() * a.getY();
res.set( res.getX() + (a.getX() + b.getX()) * crossP, res.getY() + (a.getY() + b.getY()) * crossP );
}
return res.scale(1 / (6 * getArea()));
}
float toxi::geom::Polygon2D::getArea()
{
double area = 0;
for (int i = 0, num = vertices.size(); i < num; i++)
{
Vec2D a = vertices.at( i );
Vec2D b = vertices.at( ( i + 1 ) % num );
area += a.getX() * b.getY();
area -= a.getY() * b.getX();
}
area *= 0.5f;
return static_cast< float > ( area );
}
bool outOfRange(Body body) {
if (body.getXPos() < _position.getX() - _box.getSize() / 2) {
return true;
}
if (body.getXPos() > _position.getX() + _box.getSize() / 2) {
return true;
}
if (body.getYPos() < _position.getY() - _box.getSize() / 2) {
return true;
}
if (body.getYPos() > _position.getY() + _box.getSize() / 2) {
return true;
}
}
void getBodies(Body &body, double error, vector<Body> &forceBodies) {
if (_mass == 0 && _body == NULL)
return;
double s = _box.getSize();
double d = (_centerOfMass - body.getPos()).getMagnitude();
double SbyD = s/d;
// cout << "S/D: " << SbyD << endl;
if (_state == INTERNAL) {
if (SbyD < error) {
Body overallBody(_mass, 0, 0, _centerOfMass.getX(), _centerOfMass.getY());
forceBodies.push_back(overallBody);
return;
} else {
if (_NW != NULL)
_NW->getBodies(body, error, forceBodies);
if (_NE != NULL)
_NE->getBodies(body, error, forceBodies);
if (_SW != NULL)
_SW->getBodies(body, error, forceBodies);
if (_SE != NULL)
_SE->getBodies(body, error, forceBodies);
}
} else {
if (_body != NULL) {
forceBodies.push_back(*_body);
}
}
}
int getQuadrantOfBody(Body body) {
double x = body.getXPos();
double y = body.getYPos();
if (x < _position.getX()) {
if (y > _position.getY()) {
return 1;
} else {
return 3;
}
} else {
if (y > _position.getY()) {
return 2;
} else {
return 4;
}
}
}
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;
}
}
bool toxi::geom::Polygon2D::containsPoint( Vec2D & p )
{
int num = vertices.size();
int i, j = num - 1;
bool oddNodes = false;
double px = p.getX();
double py = p.getY();
for (i = 0; i < num; i++)
{
Vec2D vi = vertices.at(i);
Vec2D vj = vertices.at(j);
if (vi.getY() < py && vj.getY() >= py || vj.getY() < py && vi.getY() >= py)
{
if (vi.getX() + (py - vi.getY() ) / (vj.getY() - vi.getY()) * (vj.getX() - vi.getX()) < px)
{
oddNodes = !oddNodes;
}
}
j = i;
}
return oddNodes;
}
void print() {
cout << "Entering Print" << endl;
if (_state == INTERNAL) {
cout << "Internal" << endl;
if (_body == NULL) {
cout << "NO Body here" << endl;
cout << "Mass of internal node " << _mass << endl;
cout << "Center of Mass " << _centerOfMass.getX() << " " << _centerOfMass.getY() << endl;
} else {
cout << _body->getMass() << endl;
}
}
else {
cout << "External" << endl;
if (_body == NULL) {
cout << "NO Body here" << endl;
} else {
cout << _body->getMass() << endl;
}
}
if (_NW != NULL) {
cout << "Printing NW" << endl;
_NW->print();
}
if (_NE != NULL) {
cout << "Printing NE" << endl;
_NE->print();
}
if (_SW != NULL) {
cout << "Printing SW" << endl;
_SW->print();
}
if (_SE != NULL) {
cout << "Printing SE" << endl;
_SE->print();
}
cout << "Leaving Print" << endl;
}
bool toxi::geom::Polygon2D::toOutLine()
{
int corners = vertices.size();
int maxSegs = corners * 3;
std::vector<Vec2D> newVerts;
std::vector< Vec2D > segments( maxSegs );
std::vector< Vec2D > segEnds( maxSegs );
std::vector< double > segAngles( maxSegs );
//Vec2D * segments;
//segments = ( Vec2D* ) malloc( sizeof( Vec2D ) * maxSegs);
//Vec2D * segEnds;
//segEnds = (Vec2D * ) malloc(sizeof( Vec2D ) * maxSegs );
//float * segAngles;
//segAngles = (float * ) malloc( sizeof( float ) * maxSegs );
//Vec2D[] segments = new Vec2D[maxSegs];
//Vec2D[] segEnds = new Vec2D[maxSegs];
//float[] segAngles = new float[maxSegs];
Vec2D start = vertices.at(0);
double lastAngle = toxi::math::MathUtils::PI;
float a, b, c, d, e, f;
double angleDif, bestAngleDif;
int i, j = corners - 1, segs = 0;
if (corners > maxSegs) {
return false;
}
// 1,3. Reformulate the polygon as a set of line segments, and choose a
// starting point that must be on the perimeter.
for (i = 0; i < corners; i++) {
Vec2D pi = vertices.at(i);
Vec2D pj = vertices.at(j);
if (!( pi == pj )) {
segments[segs] = pi;
segEnds[segs++] = pj;
}
j = i;
if (pi.getY() > start.getY() || (pi.getY() == start.getY() && pi.getX() < start.getX())) {
start.set( pi);
}
}
if (segs == 0) {
return false;
}
// 2. Break the segments up at their intersection points.
for (i = 0; i < segs - 1; i++) {
for (j = i + 1; j < segs; j++) {
Line2D li = toxi::geom::Line2D( segments[i], segEnds[i]);
Line2D lj = toxi::geom::Line2D( segments[j], segEnds[j]);
LineIntersection isec = li.intersectLine( lj );
if (isec.getType() == toxi::geom::LineIntersection::Type::INTERSECTING) {
Vec2D ipos = isec.getPos();
if (!( ipos == segments[i] ) && !( ipos == segEnds[i])) {
if (segs == maxSegs) {
return false;
}
segments[segs] = segments[i];
segEnds[segs++] = ipos;
segments[i] = ipos;
}
if (!( ipos == segments[j] ) && !( ipos == segEnds[ j ] ) ) {
if (segs == maxSegs) {
return false;
}
segments[segs] = segments[j];
segEnds[segs++] = ipos;
segments[j] = ipos;
}
}
}
}
// Calculate the angle of each segment.
for (i = 0; i < segs; i++) {
segAngles[i] = segEnds[i].sub( segments[i] ).positiveHeading();
}
// 4. Build the perimeter polygon.
c = static_cast< float > ( start.getX() );
d = static_cast< float > ( start.getY() );
a = c - 1;
b = d;
e = 0;
f = 0;
newVerts.push_back(Vec2D(c, d));
corners = 1;
while (true) {
bestAngleDif = toxi::math::MathUtils::TWO_PI;
for (i = 0; i < segs; i++) {
if (segments[i].getX() == c && segments[i].getY() == d
&& (segEnds[i].getX() != a || segEnds[i].getY() != b)) {
angleDif = lastAngle - segAngles[i];
while (angleDif >= toxi::math::MathUtils::TWO_PI) {
angleDif -= toxi::math::MathUtils::TWO_PI;
}
while (angleDif < 0) {
angleDif += toxi::math::MathUtils::TWO_PI;
}
if (angleDif < bestAngleDif) {
bestAngleDif = angleDif;
e = static_cast< float > ( segEnds[i].getX() );
f = static_cast< float > ( segEnds[i].getY() );
}
}
if (segEnds[i].getX() == c && segEnds[i].getY() == d
&& (segments[i].getX() != a || segments[i].getY() != b)) {
angleDif = lastAngle - segAngles[i] + toxi::math::MathUtils::PI;
while (angleDif >= toxi::math::MathUtils::TWO_PI) {
angleDif -= toxi::math::MathUtils::TWO_PI;
}
while (angleDif < 0) {
angleDif += toxi::math::MathUtils::TWO_PI;
}
if (angleDif < bestAngleDif) {
bestAngleDif = angleDif;
e = static_cast< float > ( segments[i].getX() );
f = static_cast< float > ( segments[i].getY() );
}
}
}
if (corners > 1 && c == newVerts.at(0).getX() && d == newVerts.at(0).getY()
&& e == newVerts.at(1).getX() && f == newVerts.at(1).getY()) {
corners--;
vertices = newVerts;
return true;
}
if (bestAngleDif == toxi::math::MathUtils::TWO_PI || corners == maxSegs) {
return false;
}
lastAngle -= bestAngleDif + toxi::math::MathUtils::PI;
newVerts.push_back(Vec2D(e, f));
corners++;
a = c;
b = d;
c = e;
d = f;
}
}
Vec2D operator/(const Vec2D &Vec, const double scalar) {
if (scalar == 0) {
cout << "ERROR" << endl;
}
return Vec2D(Vec.getX() / scalar, Vec.getY() / scalar);
}
Vec2D operator*(const double scalar, const Vec2D &Vec) {
return Vec2D(Vec.getX() * scalar, Vec.getY() * scalar);
}
Vec2D operator-(const Vec2D &Vec1, const Vec2D &Vec2) {
return Vec2D(Vec1.getX() - Vec2.getX(), Vec1.getY() - Vec2.getY());
}
double getYPos() const {
return _pos.getY();
}
double getYVel() const {
return _vel.getY();
}
Vec2D operator/(const Vec2D &Vec, const double scalar) {
return Vec2D(Vec.getX() / scalar, Vec.getY() / scalar);
}
bool insertBody(Body body) {
// cout << "In insert" << endl;
if (outOfRange(body)) {
return false;
}
if ((_mass == 0 && isExternal())) {
// cout << "Keeping body here" << endl;
_mass += body.getMass();
_centerOfMass = _centerOfMass + body.getPos();
//_body = new Body(body);
return true;
}
// updateCenterOfMass(body);
Body temp(_mass, 0, 0, _centerOfMass.getX(), _centerOfMass.getY());
if (isExternal()) {
// cout << "is external" << endl;
_mass = 0;
_centerOfMass = Vec2D(0,0);
int quad = getQuadrantOfBody(temp);
if (quad == 1) {
if (_NW == NULL)
_NW = new TreeNode(_box.getQuadrant(1));
_NW->insertBody(temp);
}
if (quad == 2) {
if (_NE == NULL)
_NE = new TreeNode(_box.getQuadrant(2));
_NE->insertBody(temp);
}
if (quad == 3) {
if (_SW == NULL)
_SW = new TreeNode(_box.getQuadrant(3));
_SW->insertBody(temp);
}
if (quad == 4) {
if (_SE == NULL)
_SE = new TreeNode(_box.getQuadrant(4));
_SE->insertBody(temp);
}
// cout << "is external 2 " << endl;
}
int quad = getQuadrantOfBody(body);
if (quad == 1) {
if (_NW == NULL)
_NW = new TreeNode(_box.getQuadrant(1));
_NW->insertBody(body);
}
if (quad == 2) {
if (_NE == NULL)
_NE = new TreeNode(_box.getQuadrant(2));
_NE->insertBody(body);
}
if (quad == 3) {
if (_SW == NULL)
_SW = new TreeNode(_box.getQuadrant(3));
_SW->insertBody(body);
}
if (quad == 4) {
if (_SE == NULL)
_SE = new TreeNode(_box.getQuadrant(4));
_SE->insertBody(body);
}
if (_NW != NULL) {
_mass += _NW->_mass;
_centerOfMass = _centerOfMass + _NW->_mass * _NW->_centerOfMass;
}
if (_NE != NULL) {
_mass += _NE->_mass;
_centerOfMass = _centerOfMass + _NE->_mass * _NE->_centerOfMass;
}
if (_SW != NULL) {
_mass += _SW->_mass;
_centerOfMass = _centerOfMass + _SW->_mass * _SW->_centerOfMass;
}
if (_SE != NULL) {
_mass += _SE->_mass;
_centerOfMass = _centerOfMass + _SE->_mass * _SE->_centerOfMass;
}
if (_mass != 0) {
_centerOfMass = _centerOfMass / _mass;
}
// updateCenterOfMass(_mass, _centerOfMass);
// cout << "Leaving insert" << endl;
return false;
}