CH Skeleton

Main.cc

#include <MathGeoLib/MathGeoLib.h>
#include <Windows.h>

#include <deque>
#include <set>
#include <sstream>
#include <vector>

namespace skeleton
{
	static int const parents[] = {
		-1, 0, 0, 2, 2, 0, 5, 6, 7, 8, 9, 8, 11, 8, 13, 8, 15, 8, 17, 18, 8,
		20, 21, 8, 23, 24, 8, 26, 27, 6, 29, 30, 31, 31, 29, 34, 35, 29, 37,
		38, 6, 40, 41, 41, 43, 41, 45, 41, 47, 41, 49, 41, 51, 52, 41, 54, 55,
		41, 57, 58, 41, 60, 61, 5, 63, 64, 65, 66, 5, 68, 69, 70, 71, -1, -1
	};

	static Quat const rotations[] = {
		Quat(-0.000000f, -0.000000f, -0.707107f, -0.707107f),
		Quat(-0.707107f, 0.000000f, 0.000000f, -0.707107f),
		Quat(-0.707107f, 0.000000f, 0.000000f, -0.707107f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(-0.707107f, 0.000000f, 0.000000f, -0.707107f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(0.415923f, 0.550533f, 0.550533f, -0.469931f),
		Quat(0.106843f, 0.139214f, 0.105226f, -0.978842f),
		Quat(-0.073907f, -0.116461f, 0.005454f, -0.990427f),
		Quat(0.090957f, 0.111799f, -0.020591f, -0.989345f),
		Quat(-0.067679f, -0.081889f, 0.007504f, -0.994313f),
		Quat(0.089286f, 0.106717f, -0.069881f, -0.987804f),
		Quat(-0.068415f, -0.074695f, 0.006284f, -0.994837f),
		Quat(0.446193f, 0.568781f, 0.232715f, -0.650572f),
		Quat(0.075134f, 0.244210f, -0.012608f, -0.966725f),
		Quat(-0.038319f, -0.051863f, -0.069468f, -0.995498f),
		Quat(0.202669f, 0.217724f, -0.006216f, -0.954716f),
		Quat(0.023562f, 0.035960f, -0.036776f, -0.998398f),
		Quat(-0.078539f, -0.090396f, 0.118480f, -0.985709f),
		Quat(0.198319f, 0.311978f, -0.021106f, -0.928921f),
		Quat(0.053060f, 0.099975f, -0.045888f, -0.992514f),
		Quat(-0.042897f, -0.054131f, -0.013337f, -0.997523f),
		Quat(0.194316f, 0.230672f, -0.025574f, -0.953089f),
		Quat(0.042408f, 0.049439f, -0.002648f, -0.997873f),
		Quat(0.623963f, 0.768304f, -0.118832f, -0.079113f),
		Quat(-0.193205f, -0.745234f, 0.122160f, -0.626398f),
		Quat(-0.069681f, -0.122536f, -0.030534f, -0.989544f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(-0.259674f, -0.000001f, -0.000001f, -0.965696f),
		Quat(0.259675f, 0.000001f, 0.000001f, -0.965696f),
		Quat(0.390864f, 0.487940f, 0.487943f, -0.609141f),
		Quat(0.390847f, -0.487957f, -0.487960f, -0.609124f),
		Quat(0.529471f, 0.499133f, 0.499132f, -0.470524f),
		Quat(0.000809f, -0.006094f, 0.117059f, -0.993106f),
		Quat(-0.047255f, -0.059268f, -0.008745f, -0.997085f),
		Quat(0.526721f, -0.499074f, -0.499072f, -0.473727f),
		Quat(0.011762f, -0.001958f, -0.184538f, -0.982753f),
		Quat(-0.051937f, 0.070563f, 0.063728f, -0.994114f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(0.406538f, -0.556176f, -0.556176f, -0.464826f),
		Quat(-0.028459f, -0.054878f, -0.707636f, -0.703868f),
		Quat(0.101956f, -0.136638f, -0.096937f, -0.980581f),
		Quat(-0.069591f, 0.109660f, -0.001015f, -0.991529f),
		Quat(0.090271f, -0.113005f, 0.033373f, -0.988922f),
		Quat(-0.066092f, 0.079969f, -0.007371f, -0.994577f),
		Quat(0.090293f, -0.110881f, 0.065528f, -0.987552f),
		Quat(-0.068487f, 0.075782f, 0.004370f, -0.994760f),
		Quat(0.439782f, -0.576388f, -0.223868f, -0.651347f),
		Quat(0.075132f, -0.244213f, 0.012609f, -0.966725f),
		Quat(-0.037478f, 0.050855f, 0.045040f, -0.996986f),
		Quat(0.197312f, -0.218935f, 0.066527f, -0.953262f),
		Quat(0.032543f, -0.037578f, 0.016186f, -0.998632f),
		Quat(-0.062673f, 0.072374f, -0.114087f, -0.988847f),
		Quat(0.183201f, -0.292125f, 0.027927f, -0.938254f),
		Quat(0.055903f, -0.104575f, 0.047068f, -0.991828f),
		Quat(-0.036568f, 0.048446f, 0.026355f, -0.997808f),
		Quat(0.191127f, -0.227994f, 0.019321f, -0.954524f),
		Quat(0.038750f, -0.049455f, 0.014312f, -0.997922f),
		Quat(0.614361f, -0.776223f, 0.120330f, -0.074555f),
		Quat(-0.193205f, 0.745239f, -0.122162f, -0.626392f),
		Quat(-0.069681f, 0.122535f, 0.030534f, -0.989544f),
		Quat(0.500001f, 0.499999f, 0.499999f, -0.500000f),
		Quat(0.449504f, -0.449506f, 0.545845f, -0.545843f),
		Quat(0.000000f, -0.986198f, 0.165569f, -0.000000f),
		Quat(0.672762f, 0.000002f, -0.000002f, -0.739859f),
		Quat(-0.000000f, 0.993127f, -0.117040f, -0.000094f),
		Quat(0.500000f, -0.500000f, -0.500000f, -0.500000f),
		Quat(0.449505f, 0.449505f, -0.545843f, -0.545844f),
		Quat(0.000000f, -0.986198f, 0.165569f, -0.000000f),
		Quat(0.672762f, 0.000002f, -0.000003f, -0.739859f),
		Quat(-0.000000f, 0.993127f, -0.117040f, -0.000094f),
		Quat(-0.000000f, -0.000000f, -0.000000f, -1.000000f),
		Quat(0.000000f, 0.000000f, 1.000000f, -0.000000f)
	};

	static float3 const scales[] = {
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f),
		float3(1.000000f, 1.000000f, 1.000000f)
	};

	static float3 const translations[] = {
		float3(0.000000f, 0.000000f, 3.772637f),
		float3(0.000001f, 1.619717f, 1.799189f),
		float3(0.000000f, 0.216972f, 2.160860f),
		float3(0.000000f, 0.580199f, 0.000000f),
		float3(-0.000000f, 1.717301f, -0.315251f),
		float3(0.000000f, 0.004065f, -0.267182f),
		float3(-0.000000f, 0.733671f, -0.212907f),
		float3(3.598860f, 1.707698f, 0.063392f),
		float3(0.000000f, 0.000000f, 0.000000f),
		float3(0.274402f, 0.744331f, 0.011353f),
		float3(0.008036f, 0.647461f, 0.008288f),
		float3(0.006664f, 0.767578f, -0.005731f),
		float3(0.007424f, 0.699336f, 0.008175f),
		float3(-0.254985f, 0.719625f, -0.023035f),
		float3(-0.002464f, 0.567533f, 0.007302f),
		float3(0.214058f, 0.095714f, -0.001298f),
		float3(0.000000f, 0.549626f, 0.000001f),
		float3(-0.254985f, 0.719625f, -0.023035f),
		float3(0.000000f, 0.236150f, 0.000000f),
		float3(0.000000f, 0.163520f, -0.000000f),
		float3(0.274402f, 0.744331f, 0.011353f),
		float3(-0.000000f, 0.240655f, 0.000001f),
		float3(-0.000000f, 0.224567f, -0.000001f),
		float3(0.006664f, 0.767578f, -0.005731f),
		float3(-0.000001f, 0.291139f, 0.000001f),
		float3(-0.000000f, 0.206105f, -0.000001f),
		float3(0.214058f, 0.095714f, -0.001298f),
		float3(-0.000000f, 0.218346f, -0.000001f),
		float3(-0.000000f, 0.183099f, 0.000000f),
		float3(-0.000000f, 1.137102f, -0.315251f),
		float3(0.000000f, 1.137102f, 0.000000f),
		float3(0.000000f, 0.500431f, -0.000001f),
		float3(0.500319f, 0.567876f, 0.193385f),
		float3(-0.500319f, 0.567876f, 0.193385f),
		float3(1.208260f, 0.701688f, 0.159978f),
		float3(-0.000001f, 1.478591f, 0.000001f),
		float3(-0.000000f, 0.941381f, -0.000000f),
		float3(-1.208260f, 0.701688f, 0.159978f),
		float3(-0.001480f, 1.644637f, -0.000043f),
		float3(-0.000002f, 0.836343f, 0.000001f),
		float3(-3.628772f, 1.707699f, 0.147991f),
		float3(0.000000f, 0.000000f, 0.000000f),
		float3(0.066763f, 0.300523f, -0.150182f),
		float3(-0.262404f, 0.685064f, 0.012876f),
		float3(-0.007573f, 0.693138f, 0.008666f),
		float3(-0.008183f, 0.721906f, -0.005541f),
		float3(-0.007368f, 0.714458f, 0.007988f),
		float3(0.251533f, 0.676046f, -0.024103f),
		float3(0.012450f, 0.569442f, 0.008271f),
		float3(-0.261772f, 0.137253f, 0.000559f),
		float3(0.000001f, 0.549627f, 0.000001f),
		float3(0.251533f, 0.676046f, -0.024103f),
		float3(-0.000001f, 0.242143f, -0.000000f),
		float3(-0.000001f, 0.162920f, -0.000001f),
		float3(-0.262404f, 0.685064f, 0.012876f),
		float3(-0.000001f, 0.280412f, 0.000000f),
		float3(-0.000001f, 0.230313f, -0.000001f),
		float3(-0.008183f, 0.721906f, -0.005541f),
		float3(0.000000f, 0.306233f, 0.000001f),
		float3(-0.000002f, 0.206570f, -0.000001f),
		float3(-0.261772f, 0.137253f, 0.000559f),
		float3(-0.000000f, 0.218346f, -0.000001f),
		float3(-0.000000f, 0.183099f, 0.000000f),
		float3(0.000000f, -0.013518f, 0.004065f),
		float3(0.000000f, 0.518800f, 0.000000f),
		float3(0.000000f, 1.184893f, -0.000000f),
		float3(0.000000f, 1.727772f, -0.000000f),
		float3(0.000000f, 1.058760f, 0.000003f),
		float3(0.000000f, -0.013518f, 0.004065f),
		float3(0.000002f, 0.518801f, 0.000000f),
		float3(-0.000001f, 1.184889f, 0.000001f),
		float3(-0.000002f, 1.727769f, -0.000002f),
		float3(0.000001f, 1.058760f, 0.000006f),
		float3(-0.336646f, 1.590711f, 5.196055f),
		float3(-0.336646f, -1.590711f, 5.196055f)
	};
	static size_t const count = sizeof(parents)/sizeof(parents[0]);
}

void WriteString(HANDLE h, std::string const& s)
{
	DWORD nWritten = 0;
	WriteFile(h, s.c_str(), s.size(), &nWritten, 0);
}

std::string SpacedString(float3 p)
{
	std::ostringstream oss;
	oss << p.x << " " << p.y << " " << p.z;
	std::string out = oss.str();
	return out;
}

void WriteBone(HANDLE h, float3x4 From, float3x4 To)
{
	float const s = 0.10f;
	float3 from = From.TransformPos(float3::zero);
	float3 fromX = From.TransformPos(s * float3::unitX);
	float3 fromZ = From.TransformPos(s * float3::unitZ);
	float3 to = To.TransformPos(float3::zero);
	float3 toX = To.TransformPos(s * float3::unitX);
	float3 toZ = To.TransformPos(s * float3::unitZ);
	std::ostringstream oss;
	oss << "v " << SpacedString(from) << "\n"
		<< "v " << SpacedString(to) << "\n"
		<< "v " << SpacedString(toX) << "\n"
		<< "v " << SpacedString(toZ) << "\n"
		<< "f -4 -3 -2" << "\n"
		<< "f -4 -2 -1" << "\n"
		<< "f -4 -1 -3" << "\n"
		;

	WriteString(h, oss.str());
}

std::vector<int> TopologicalSort(int const* parents, size_t count)
{
	std::vector<int> E(parents, parents + count);
	std::vector<int> L;
	L.reserve(count);
	std::set<int> S;
	for (size_t i = 0; i < count; ++i) {
		if (E[i] == -1) {
			S.insert(i);
		}
	}
	while (!S.empty()) {
		int n = *S.begin();
		S.erase(S.begin());
		L.push_back(n);
		for (size_t i = 0; i < count; ++i) {
			if (E[i] == n) {
				E[i] = -1;
				S.insert(i);
			}
		}
	}
	return L;
}

int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int)
{
	float3x4* matrices = new float3x4[skeleton::count]();
	std::vector<int> order = TopologicalSort(skeleton::parents, skeleton::count);
	for (int i : order) {
		float3x4 S = float3x4::Scale(skeleton::scales[i]);
		float3x4 R = skeleton::rotations[i].ToFloat3x4();
		float3x4 T = float3x4::Translate(skeleton::translations[i]);
		float3x4 M = T*R*S;
		int parent = skeleton::parents[i];
		if (parent != -1) {
			M = matrices[parent] * M;
		}
		matrices[i] = M;
	}
	HANDLE h = CreateFileA("skeleton.obj", GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, CREATE_ALWAYS, 0, 0);
	WriteString(h, "g skeleton\n");
	float3x4 W = float3x4::RotateX(-math::pi / 2.0f);
	for (size_t i = 0; i < skeleton::count; ++i) {
		int parent = skeleton::parents[i];
		if (parent != -1) {
			WriteBone(h, W * matrices[parent], W * matrices[i]);
		}
	}
	CloseHandle(h);
	return 0;
}