RigidBody

#include <stdio.h>

#include "rigidbody.h"

// Vertices
Vertices vertices(size_t len) {
    Vertices result;
    result.length = len;
    result.points = (Point*)malloc(len*sizeof(Point));
    return result;
}
void set(Vertices *verts, size_t index, float x, float y) {
    if (index < verts->length) {
        verts->points[index].x = x;
        verts->points[index].y = y;
    }
    else perror("out of bounds");
}

Point get(Vertices *verts, size_t index) {
    if (index < verts->length) {
        Point result = {verts->points[index].x, verts->points[index].y};
        return result;
    }
    else perror("out of bounds");
}

void freeVerts(Vertices *verts) {
    free(verts->points);
}

// Polygon
Point findCentroid(Polygon *pg) {
    float xi, yi, xp, yp, A=0; // x_i, y_i, x_(i+1), y_(i+1), A
    Point C = {0,0}; // C_x, C_y
    for(int i = 0; i <= pg->vertices.length-1; i++) {
        //printf("(%f, %f)\n", C.x, C.y);
        xi = pg->vertices.points[i].x;
        yi = pg->vertices.points[i].y;
        if (i == pg->vertices.length-1) {
            xp = pg->vertices.points[0].x;
            yp = pg->vertices.points[0].y;
        }
        else {
            xp = pg->vertices.points[i+1].x;
            yp = pg->vertices.points[i+1].y;
        }
        C.x += (xi+xp)*(xi*yp - xp*yi);
        C.y += (yi+yp)*(xi*yp - xp*yi);
        A   += (xi*yp - xp*yi);
        //printf("A:%f\n", A);
    }
    A   *= 0.5f;
    C.x *= 1/(6*A);
    C.y *= 1/(6*A);
    pg->centroid = C;
    return C;
}

void freePoly(Polygon *pg) {
    freeVerts(&pg->vertices);
}

// TEST
int main(void) {
    Polygon p = {.vertices = vertices(4)};
    set(&p.vertices,0, 1,0);
    set(&p.vertices,1, 0,1);
    set(&p.vertices,2, -1,0);
    set(&p.vertices,3, 0,-1);
    findCentroid(&p);
    printf("(%f, %f)", p.centroid.x, p.centroid.y);
    //printf("%zu, %f", p.vertices.length, get(&p.vertices,0).x);
    
    freePoly(&p);
    return 0;
}