private void PublishIdealVelocities()
{
/*Ubii.TopicData.TopicData topicData = new Ubii.TopicData.TopicData { TopicDataRecord = new Ubii.TopicData.TopicDataRecord {
* Topic = GetTopicTargetVelocities(),
* Object3DList = new Ubii.DataStructure.Object3DList()
* } };*/
Ubii.TopicData.TopicDataRecord record = new Ubii.TopicData.TopicDataRecord
{
Topic = ubiiComponentAvatarForceControl.GetTopicTargetVelocities(),
Object3DList = new Ubii.DataStructure.Object3DList()
};
foreach (KeyValuePair <HumanBodyBones, Transform> entry in mapBone2TargetTransform)
{
HumanBodyBones bone = entry.Key;
Transform targetTransform = entry.Value;
UbiiPose3D currentPose;
if (mapBone2CurrentPose.TryGetValue(bone, out currentPose))
{
Vector3 linearVelocity = scalingFactorsVelocities.x * GetIdealLinearVelocity(currentPose.position, targetTransform.position + manualPositionOffset);
Vector3 angularSpeed = scalingFactorsVelocities.y * GetIdealAngularVelocity(currentPose.rotation, targetTransform.rotation);
Ubii.DataStructure.Object3D ubiiObject3D = new Ubii.DataStructure.Object3D
{
Id = bone.ToString(),
Pose = new Ubii.DataStructure.Pose3D {
}
};
if (this.publishLinearVelocity)
{
ubiiObject3D.Pose.Position = new Ubii.DataStructure.Vector3
{
X = linearVelocity.x,
Y = linearVelocity.y,
Z = linearVelocity.z
};
}
if (this.publishAngularVelocity)
{
ubiiObject3D.Pose.Euler = new Ubii.DataStructure.Vector3
{
X = angularSpeed.x,
Y = angularSpeed.y,
Z = angularSpeed.z
};
}
record.Object3DList.Elements.Add(ubiiObject3D);
}
}
ubiiNode.Publish(record);
}
private void PublishCurrentPosesList()
{
Ubii.TopicData.TopicDataRecord record = new Ubii.TopicData.TopicDataRecord
{
Topic = this.ubiiSpecs.Topic,
Object3DList = new Ubii.DataStructure.Object3DList()
};
Dictionary <HumanBodyBones, Rigidbody> mapBone2Rigidbody = avatarPhysicsManager.GetMapBone2Rigidbody();
foreach (KeyValuePair <HumanBodyBones, Rigidbody> entry in mapBone2Rigidbody)
{
Transform currentTransform = entry.Value.transform;
record.Object3DList.Elements.Add(new Ubii.DataStructure.Object3D
{
Id = entry.Key.ToString(),
Pose = new Ubii.DataStructure.Pose3D
{
Position = new Ubii.DataStructure.Vector3
{
X = currentTransform.position.x,
Y = currentTransform.position.y,
Z = currentTransform.position.z
},
Quaternion = new Ubii.DataStructure.Quaternion
{
X = currentTransform.rotation.x,
Y = currentTransform.rotation.y,
Z = currentTransform.rotation.z,
W = currentTransform.rotation.w
}
}
});
}
ubiiNode.Publish(record);
}
private void PublishTopicDataIKTargets()
{
Ubii.TopicData.TopicDataRecord record = new Ubii.TopicData.TopicDataRecord {
Topic = GetTopicIKTargets(),
Object3DList = new Ubii.DataStructure.Object3DList()
};
foreach (IK_TARGET ikTarget in Enum.GetValues(typeof(IK_TARGET)))
{
Vector3 ikTargetPosition = ikTargetsManager.GetIkTargetPosition(ikTarget);
Quaternion ikTargetRotation = ikTargetsManager.GetIkTargetRotation(ikTarget);
record.Object3DList.Elements.Add(new Ubii.DataStructure.Object3D
{
Id = ikTarget.ToString(),
Pose = new Ubii.DataStructure.Pose3D
{
Position = new Ubii.DataStructure.Vector3
{
X = ikTargetPosition.x,
Y = ikTargetPosition.y,
Z = ikTargetPosition.z
},
Quaternion = new Ubii.DataStructure.Quaternion
{
X = ikTargetRotation.x,
Y = ikTargetRotation.y,
Z = ikTargetRotation.z,
W = ikTargetRotation.w
}
}
});
}
ubiiNode.Publish(record);
}