//this function called by The UDP thread
public void copyData(ActorFrameData data)
{
lock (frameDataBuffer)
{
data.CopyTo(ref frameDataBuffer);
dataPending = true;
}
}
//将数据拷贝到other中
public void CopyTo(ref ActorFrameData other)
{
other.motionVenusProtoVer = motionVenusProtoVer;
other.strActorName = string.Copy(strActorName);
other.suitNumber = suitNumber;
other.suitType = suitType;
other.frameNumber = frameNumber;
other.boneCount = boneCount;
other.boneRotQuat = new Dictionary <byte, Quaternion>(boneRotQuat);
other.bonePositions = new Dictionary <byte, Vector3>(bonePositions);
other.sensorAccels = new Dictionary <byte, Vector3>(sensorAccels);
other.sensorMags = new Dictionary <byte, Vector3>(sensorMags);
other.sensorGyros = new Dictionary <byte, Vector3>(sensorGyros);
other.isBoneRotQuat = isBoneRotQuat;
other.isBoneRotEuler = isBoneRotEuler;
other.isBonePositions = isBonePositions;
other.isSensorAccels = isSensorAccels;
other.isSensorMag = isSensorMag;
other.isSensorGyro = isSensorGyro;
}
//接收数据的工作线程
void mcReceiveData()
{
//接收广播
IPEndPoint endpoint = new IPEndPoint(IPAddress.Any, 0);
ActorFrameData frameDataTemp = new ActorFrameData();
while (true)
{
try
{
byte[] data = udpReceiver.Receive(ref endpoint);
if (data.Length == UI_Calibrate.STANDARD_UDP_STATUS_BUFFER_SIZE)
{
Calibrate.status = BitConverter.ToInt32(data, 0);
Calibrate.second = BitConverter.ToInt32(data, Marshal.SizeOf(Calibrate.status));
}
else
{
int dataErro = frameDataTemp.deComposeData(data, BonePosition, BoneEuler, BoneQuat, SensorAccel, SensorMag, SensorGyro);
if (dataErro != 0)
{
Debug.Log("Data Erro:" + dataErro);
continue;
}
}
}
catch (Exception e)
{
Debug.Log(e.ToString());
}
foreach (FoheartModel model in PlayerList)
{
if (string.Equals(model.ActorName, frameDataTemp.strActorName))
{
model.copyData(frameDataTemp);
}
}
}
}
void Awake()
{
CheckCondition();
mapBoneIndex = new Dictionary <uint, string>();
m_mapNameAndBone = new Dictionary <string, Transform>();
mapBoneInitAxisTrans = new Dictionary <uint, initAxisAndRot>();
frameDataBuffer = new ActorFrameData();
loadBoneMaps();
initNameAndBone();
ChildAwake();
Transform HipsStartTrans = FindBone(0);
//Debug.Log("Start Location:"+HipsStartTrans.position.x+" "+HipsStartTrans.position.y+" "+HipsStartTrans.position.z);
if (HipsStartTrans == null)
{
Debug.LogError("Please check Config Name is Right or ConnectId's name is valid in Model!");
}
//SetLocationOffset = SetLocationOffset;// +new Vector3(HipsStartTrans.position.x, HipsStartTrans.position.y, HipsStartTrans.position.z);
// SetLocationOffset = new Vector3(HipsStartTrans.position.x, HipsStartTrans.position.y, HipsStartTrans.position.z);
}
public virtual void applyBoneTransRot(ActorFrameData data)
{
Transform BoneHip = FindBone(0);
if (BoneHip)
{
Vector3 LocInUnity = new Vector3();
if (data.isBonePositions)
{
LocInUnity.Set(
((-data.bonePositions[0].x) + (LocationOffsetXYZ.x)) * LocationScaleXYZ.x,
((data.bonePositions[0].z) + (LocationOffsetXYZ.y)) * LocationScaleXYZ.y,
((-data.bonePositions[0].y) + (LocationOffsetXYZ.z)) * LocationScaleXYZ.z
);
}
else
{
LocInUnity.Set(
(0 + (LocationOffsetXYZ.x)) * LocationScaleXYZ.x,
(0 + (LocationOffsetXYZ.y)) * LocationScaleXYZ.y,
(0.96f + (LocationOffsetXYZ.z)) * LocationScaleXYZ.z
);
}
if (data.isBonePositions)
{
for (int i = 0; i < 23; i++)
{
Byte ind = (Byte)i;
BonesLocation[i].Set(data.bonePositions[ind].x, data.bonePositions[ind].x, data.bonePositions[ind].x);
}
}
if (data.isSensorAccels)
{
for (int i = 0; i < 23; i++)
{
Byte ind = (Byte)i;
//BonesLocation [i].Set (data.bonePositions[ind].x,data.bonePositions[ind].x,data.bonePositions[ind].x);
SensorAccel[i].Set(data.sensorAccels[ind].x, data.sensorAccels[ind].y, data.sensorAccels[ind].z);
}
}
if (data.isSensorMag)
{
for (int i = 0; i < 23; i++)
{
Byte ind = (Byte)i;
//BonesLocation [i].Set (data.bonePositions[ind].x,data.bonePositions[ind].x,data.bonePositions[ind].x);
SensorMag[i].Set(data.sensorMags[ind].x, data.sensorMags[ind].y, data.sensorMags[ind].z);
}
}
if (data.isSensorGyro)
{
for (int i = 0; i < 23; i++)
{
Byte ind = (Byte)i;
//BonesLocation [i].Set (data.bonePositions[ind].x,data.bonePositions[ind].x,data.bonePositions[ind].x);
SensorGyro[i].Set(data.sensorGyros[ind].x, data.sensorGyros[ind].y, data.sensorGyros[ind].z);
}
}
if (FixHipsLocation)
{
}
else
{
BoneHip.transform.position = LocInUnity;
}
}
else
{
print("[ERROR] Do not find HIP, can not set actor location.");
return;
}
foreach (var BoneR in data.boneRotQuat)
{
Transform BoneT = FindBone(BoneR.Key);
if (BoneT)
{
var BR = BoneR.Value;
float[] remoteRot = new float[3] {
BR.x, BR.y, BR.z
};
float[] transRot = new float[3];
for (int i = 0; i < transRot.Length; i++)
{
bool isNeg;
if (mapBoneInitAxisTrans[BoneR.Key].xyz[i] < 0)
{
isNeg = true;
}
else
{
isNeg = false;
}
int tempIndex = isNeg ? (-mapBoneInitAxisTrans[BoneR.Key].xyz[i]) - 1 : mapBoneInitAxisTrans[BoneR.Key].xyz[i] - 1;
transRot[i] = remoteRot[tempIndex] * (isNeg ? -1 : 1);
}
Quaternion convQuatApply = new Quaternion(transRot[0], transRot[1], transRot[2], BR.w);
Quaternion rot = Quaternion.Euler(
mapBoneInitAxisTrans[BoneR.Key].xryrzr[0],
mapBoneInitAxisTrans[BoneR.Key].xryrzr[1],
mapBoneInitAxisTrans[BoneR.Key].xryrzr[2]);
BoneT.localRotation = rot * convQuatApply;
}
}
}
