TriangleBestFitPlane.cpp

gistfile1.txt

bool calculate_best_fit_plane(const Vec3* points, u32 count, Vec3& out_center, Vec3& out_normal)
{
	if (count < 3)
	{
		return false;
	}

	Vec3 center = Vec3(0.0f);
	for (u32 i = 0; i < count; ++i)
	{
		center += points[i];
	}
	center /= (float)count;
	out_center = center;

	Vec3 normal = Vec3(0.0f);
	const Vec3* prev = &points[count - 1];
	for (u32 i = 0; i < count; ++i)
	{
		const Vec3* curr = &points[i];
		normal.x += (prev->z + curr->z) * (prev->y - curr->y);
		normal.y += (prev->x + curr->x) * (prev->z - curr->z);
		normal.z += (prev->y + curr->y) * (prev->x - curr->x);
		prev = curr;
	}

	out_normal = normalize(normal);

	return true;
}