#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;
}
#ifndef RIGIDBODY_H
#define RIGIDBODY_H
#include <stdlib.h>
// Vec2 / Point
typedef struct {
float x,y;
} Vec2;
typedef Vec2 Point;
// Vertices
typedef struct {
size_t length;
Point *points;
} Vertices;
Vertices vertices(size_t len);
void set(Vertices *verts, size_t index, float x, float y);
void freeVerts(Vertices *verts);
// Polygon
typedef struct {
Point centroid;
Vertices vertices;
} Polygon;
#define Polygon(verts) {.vertices = verts}
void freePoly(Polygon *pg);
// RigidBody
typedef struct {
Point position;
Polygon shape;
} RigidBody;
Point findCentroid(Polygon *f);
#endif
