JD2022-TU1/main/extern/Camcam/LIBS/Input/nataltools.cpp

#include "SCENE.h"  
//#pragma optimize("",off)
#ifdef NATAL_SUPPORT
#include "natal.h"  
typedef struct Body_
{
	XCAMERA_JOINT  BodyBones[XCAMERA_JOINT_COUNT];
	u32 Saved;
} Body;
typedef struct st_Channel_
{
	Body *p_TimeBodys;
	u32		ulNumberOfBodys;
} st_Channel;

u32 TimeInMilisecondToIndex(u32 TimeInMiliseconds)
{
	if (TimeInMiliseconds > 20 * 60 * 1000) TimeInMiliseconds = 20 * 60 * 1000;
	TimeInMiliseconds /= 30;
	return TimeInMiliseconds;
}

#define MAXCHANNELS 64
st_Channel g_Channels[MAXCHANNELS];
static u32 Init = 0;
static void InitNtlTool()
{
	Init = 1;
	memset(g_Channels,0,sizeof(g_Channels));
}
#define CHECK_INIT() if (!Init) InitNtlTool();


float CompareSkeleton(Vector *XYZ1  , Vector *XYZ2 , u32 ulNumberOfLinks, u32 *pLinks, float *Pertinences)
{
	CHECK_INIT();
	Vector GC1 = Vector(0,0,0);
	Vector GC2 = Vector(0,0,0);
	float Weight = 0;
	float DotProducts = 0;
	for (int i = 0 ; i < ulNumberOfLinks ; i ++)
	{
		Vector AV1 = (XYZ1[pLinks[0]] - XYZ1[pLinks[1]]);
		Vector AV2 = (XYZ2[pLinks[0]] - XYZ2[pLinks[1]]);
		AV1.Normalize();
		AV2.Normalize();
		DotProducts += Pertinences[i] * (AV1 * AV2);
		Weight += Pertinences[i];
		pLinks += 2;
	}
	if (Weight) DotProducts /= Weight;
	return DotProducts * DotProducts;
}

void NATALSKEL_SavePosture(u32 PlayerIndex, u32 Channel , u32 TimeInMilisecond)
{
	CHECK_INIT();
	if (Channel >= MAXCHANNELS) return;
	TimeInMilisecond = TimeInMilisecondToIndex(TimeInMilisecond);
	if (g_Channels[Channel].ulNumberOfBodys <= TimeInMilisecond)
	{
		u32 PrevuousulNumberOfBodys = g_Channels[Channel].ulNumberOfBodys;
#define GRANUL 64
		u32 NewNumber = (TimeInMilisecond + GRANUL) & ~(GRANUL-1);
		if (!g_Channels[Channel].ulNumberOfBodys)
			g_Channels[Channel].p_TimeBodys = (Body*)CC_malloc(sizeof(Body) * NewNumber);
		else
			g_Channels[Channel].p_TimeBodys = (Body*)CC_realloc(g_Channels[Channel].p_TimeBodys , sizeof(Body) * NewNumber);
		g_Channels[Channel].ulNumberOfBodys = NewNumber;
		memset(&g_Channels[Channel].p_TimeBodys[PrevuousulNumberOfBodys],0,
			sizeof(Body) * (g_Channels[Channel].ulNumberOfBodys - PrevuousulNumberOfBodys));
	}

	XCAMERA_JOINT *XYZ1 = &g_Joints[PlayerIndex * XCAMERA_JOINT_COUNT];
	g_Channels[Channel].p_TimeBodys[TimeInMilisecond].Saved = 1;
	memcpy(g_Channels[Channel].p_TimeBodys[TimeInMilisecond].BodyBones,XYZ1,sizeof(g_Channels[Channel].p_TimeBodys[TimeInMilisecond].BodyBones));
}

void NATALSKEL_LoadPosture(u32 PlayerIndex, u32 Channel , u32 TimeInMilisecond)
{
	CHECK_INIT();
	if (Channel >= MAXCHANNELS) return;
	TimeInMilisecond = TimeInMilisecondToIndex(TimeInMilisecond);
	if (g_Channels[Channel].ulNumberOfBodys == 0) return;
	if (g_Channels[Channel].ulNumberOfBodys <= TimeInMilisecond) TimeInMilisecond = g_Channels[Channel].ulNumberOfBodys - 1;
	XCAMERA_JOINT *XYZ1 = &g_Joints[PlayerIndex * XCAMERA_JOINT_COUNT];
	memcpy(XYZ1,g_Channels[Channel].p_TimeBodys[TimeInMilisecond].BodyBones,sizeof(g_Channels[Channel].p_TimeBodys[TimeInMilisecond].BodyBones));
}
u32 NATALSKEL_LoadChannel(u32 Channel , char *Filename)
{
	CHECK_INIT();
	if (Channel >= MAXCHANNELS) return 2;
	FILE *F;
	F = fopen(Filename,"rb");
	if (F)
	{
		if (g_Channels[Channel].ulNumberOfBodys)
			CC_free(g_Channels[Channel].p_TimeBodys);
		fread(&g_Channels[Channel].ulNumberOfBodys,1,4,F);
		if (g_Channels[Channel].ulNumberOfBodys)
		{
			g_Channels[Channel].p_TimeBodys = (Body*)CC_malloc(sizeof(Body) * (g_Channels[Channel].ulNumberOfBodys + GRANUL * 2));
			fread(g_Channels[Channel].p_TimeBodys,1,sizeof(Body) * g_Channels[Channel].ulNumberOfBodys,F);
		}
		fclose(F);
		return 0;
	}
	return 1;
}

u32 NATALSKEL_SaveChannel(u32 Channel , char *Filename)
{
	CHECK_INIT();
	if (Channel >= MAXCHANNELS) return 2;
	FILE *F;
	F = fopen(Filename,"wb");
	if (F)
	{
		fwrite(&g_Channels[Channel].ulNumberOfBodys,1,4,F);
		if (g_Channels[Channel].ulNumberOfBodys)
		{
			fwrite(g_Channels[Channel].p_TimeBodys,1,sizeof(Body) * g_Channels[Channel].ulNumberOfBodys,F);
		}
		fclose(F);
		return 0;
	}
	return 1;
}

float NATALSKEL_ComparePosture(u32 Channel1 , u32 TimeInMiliseconds1,u32 Channel2 , u32 TimeInMiliseconds2)
{
	CHECK_INIT();
	Vector Vert1[XCAMERA_JOINT_COUNT];
	Vector Vert2[XCAMERA_JOINT_COUNT];
	float Pertinences[g_uNumBones];
	u32 Links[g_uNumBones * 2];
	if (Channel1 >= MAXCHANNELS) return 0;
	if (Channel2 >= MAXCHANNELS) return 0;
	TimeInMiliseconds1 = TimeInMilisecondToIndex(TimeInMiliseconds1);
	TimeInMiliseconds2 = TimeInMilisecondToIndex(TimeInMiliseconds2);
	if (g_Channels[Channel1].ulNumberOfBodys <= TimeInMiliseconds1) return 0;
	if (g_Channels[Channel2].ulNumberOfBodys <= TimeInMiliseconds2) return 0;
	XCAMERA_JOINT   *gJoint1 = g_Channels[Channel1].p_TimeBodys[TimeInMiliseconds1].BodyBones;
	XCAMERA_JOINT   *gJoint2 = g_Channels[Channel2].p_TimeBodys[TimeInMiliseconds2].BodyBones;
	for ( u32 i = 0; i < XCAMERA_JOINT_COUNT; i++ )
	{
		Vert1[i] = Vector(	gJoint1[i].fX,gJoint1[i].fY,gJoint1[i].fZ);
		Vert2[i] = Vector(	gJoint2[i].fX,gJoint2[i].fY,gJoint2[i].fZ);
	}
	for ( u32 i = 0; i < g_uNumBones; i++ )
	{
        Pertinences[i] = min( g_Joints[ g_Bones[ i ].StartJoint ].fConfidence, g_Joints[ g_Bones[ i ].EndJoint ].fConfidence ) * g_Bones[ i ].Ponderation;
		Links[i * 2 + 0] = g_Bones[ i ].StartJoint;
		Links[i * 2 + 1] = g_Bones[ i ].EndJoint;
	}
	return CompareSkeleton(Vert1, Vert2, g_uNumBones, Links, Pertinences);
}

float NATAL_Compare2Players(int I1 , int I2)
{
	CHECK_INIT();
	Vector Vert1[XCAMERA_JOINT_COUNT];
	Vector Vert2[XCAMERA_JOINT_COUNT];
	float Pertinences[g_uNumBones];
	u32 Links[g_uNumBones * 2];
	for ( u32 i = 0; i < XCAMERA_JOINT_COUNT; i++ )
	{
		Vert1[i] = Vector(	g_Joints[i + I1 * XCAMERA_JOINT_COUNT].fX,g_Joints[i + I1 * XCAMERA_JOINT_COUNT].fY,g_Joints[i + I1 * XCAMERA_JOINT_COUNT].fZ);
		Vert2[i] = Vector(	g_Joints[i + I2 * XCAMERA_JOINT_COUNT].fX,g_Joints[i + I2 * XCAMERA_JOINT_COUNT].fY,g_Joints[i + I2 * XCAMERA_JOINT_COUNT].fZ);
	}
	for ( u32 i = 0; i < g_uNumBones; i++ )
	{
        Pertinences[i] = min( g_Joints[ g_Bones[ i ].StartJoint ].fConfidence, g_Joints[ g_Bones[ i ].EndJoint ].fConfidence ) * g_Bones[ i ].Ponderation;
		Links[i * 2 + 0] = g_Bones[ i ].StartJoint;
		Links[i * 2 + 1] = g_Bones[ i ].EndJoint;
	}
	return CompareSkeleton(Vert1, Vert2, g_uNumBones, Links, Pertinences);
}

#endif