///
/// @brief Default constructor
///
public SensoNetworkThread(string host, Int32 port)
{
m_port = port;
State = SensoNetworkState.SENSO_DISCONNECTED;
m_buffer = new Byte[RECV_BUFFER_SIZE];
outBuffer = new Byte[SEND_BUFFER_SIZE];
handSamples = new SensoHandData[(int)ESensoPositionType.PositionsCount];
for (int i = 0; i < (int)ESensoPositionType.PositionsCount; ++i)
{
handSamples[i] = new SensoHandData();
}
if (!IPAddress.TryParse(host, out m_ip))
{
State = SensoNetworkState.SENSO_ERROR;
Debug.LogError("SensoManager: can't parse senso driver host");
}
vibroPackets = new VibroPacket[10]; // 5 for each hand. 1-5 = right hand, 6-10 = left hand
for (int i = 0; i < vibroPackets.Length; ++i)
{
vibroPackets[i] = new VibroPacket();
}
gestures = new LinkedList <SensoHandGesture>();
GesturesCount = 0;
}
public void SetSensoPose(SensoHandData aData)
{
if (ClaviclePivot != null && aData.hasClavicle)
{
ClaviclePivot.localRotation = aData.clavicleRotation;
}
if (ShoulderPivot != null && aData.hasShoulder)
{
ShoulderPivot.localRotation = aData.shoulderRotation;
}
ElbowPivot.localRotation = aData.wristRotation;
// Wrist rotation
Quaternion wq = new Quaternion((HandType == ESensoPositionType.RightHand ? -1 : 1) * aData.wristRotation.z, (HandType == ESensoPositionType.RightHand ? 1 : -1) * aData.wristRotation.y, aData.wristRotation.x, aData.wristRotation.w);
Quaternion pq = new Quaternion((HandType == ESensoPositionType.RightHand ? -1 : 1) * aData.palmRotation.z, (HandType == ESensoPositionType.RightHand ? 1 : -1) * aData.palmRotation.y, aData.palmRotation.x, aData.palmRotation.w);
WristPivot.localRotation = (Quaternion.Inverse(wq) * pq);
//Fingers
setFingerBones(ref thumbBones, aData.fingerAngles[0], ESensoFingerType.Thumb);
setFingerBones(ref indexBones, aData.fingerAngles[1], ESensoFingerType.Index);
setFingerBones(ref middleBones, aData.fingerAngles[2], ESensoFingerType.Middle);
setFingerBones(ref thirdBones, aData.fingerAngles[3], ESensoFingerType.Third);
setFingerBones(ref littleBones, aData.fingerAngles[4], ESensoFingerType.Little);
}
public void SensoPoseChanged(SensoHandData aData)
{
latestSample = aData;
sampleChanged = true;
for (ESensoFingerType fingerType = ESensoFingerType.Thumb; fingerType <= ESensoFingerType.Little; ++fingerType)
{
var tip = GetFingerTip(fingerType);
if (tip != null)
{
tip.SetRelativePitch(aData.fingerAngles[(int)fingerType].z);
}
}
}
void Update()
{
sensoThread.UpdateData();
SensoHandData leftSample = null, rightSample = null;
if (m_rightEnabled)
{
rightSample = sensoThread.GetSample(ESensoPositionType.RightHand);
}
if (m_leftEnabled)
{
leftSample = sensoThread.GetSample(ESensoPositionType.LeftHand);
}
if (sensoHands != null)
{
foreach (var hand in sensoHands)
{
if (hand.HandType == ESensoPositionType.RightHand)
{
hand.SensoPoseChanged(rightSample);
}
else if (hand.HandType == ESensoPositionType.LeftHand)
{
hand.SensoPoseChanged(leftSample);
}
}
}
// Gestures
var gestures = sensoThread.GetGestures();
if (gestures != null)
{
for (int i = 0; i < gestures.Length; ++i)
{
if (gestures[i].Type == ESensoGestureType.PinchStart || gestures[i].Type == ESensoGestureType.PinchEnd)
{
fingerPinch(gestures[i].Hand, gestures[i].Fingers[0], gestures[i].Fingers[1], gestures[i].Type == ESensoGestureType.PinchEnd);
}
}
}
}
///
/// @brief Copy constructor
///
public SensoHandData(SensoHandData old)
{
palmPosition = old.palmPosition;
palmRotation = old.palmRotation;
wristRotation = old.wristRotation;
for (int i = 0; i < 5; ++i)
{
fingerAngles[i] = old.fingerAngles[i];
}
hasShoulder = old.hasShoulder;
if (hasShoulder)
{
shoulderRotation = old.shoulderRotation;
}
hasClavicle = old.hasClavicle;
if (hasClavicle)
{
clavicleRotation = old.clavicleRotation;
}
}
