/// <summary>
/// Creates an entity at the given position.
/// </summary>
/// <param name="position">Given position.</param>
string CreateEntityAt(Vector position)
{
Entity entity = new Entity();
entity["location"]["position"].Suggest(position);
World.Instance.Add(entity);
return entity.Guid.ToString ();
}
public void ShouldSetEntityToGroundlevel()
{
var e = new Entity();
e["avatar"]["userLogin"].Suggest("123");
e["avatarCollision"]["groundLevel"].Suggest(0f);
World.Instance.Add(e);
e["location"]["position"].Suggest(new Vector(1, 2, 3));
bool finishedComputation = false;
Vector entityPosition = new Vector(0,0,0);
ServiceBus.Instance.ComputedResult += (o, r) =>
{
if (r.AccumulatedTransformations.ContainsKey("location"))
{
finishedComputation = true;
entityPosition = (Vector)e["location"]["position"].Value;
Assert.AreEqual(entityPosition.x, 1f);
Assert.AreEqual(entityPosition.y, 0f);
Assert.AreEqual(entityPosition.z, 3f);
}
};
var delay = 25;
var pollingInterval = 100; // 100 milliseconds
// Check that the computation has actually finished correctly
Assert.That(() => finishedComputation, Is.EqualTo(true).After(delay, pollingInterval));
}
public static Vector CrossProduct(Vector vector1, Vector vector2)
{
return new Vector(
vector1.y * vector2.z - vector1.z * vector2.y,
vector1.z * vector2.x - vector1.x * vector2.z,
vector1.x * vector2.y - vector1.y * vector2.x
);
}
public static Vector AddVectors(Vector vector1, Vector vector2)
{
return new Vector(
vector1.x + vector2.x,
vector1.y + vector2.y,
vector1.z + vector2.z
);
}
public static Quat QuaternionFromAxisAngle(Vector axis, float r)
{
return new Quat(
axis.x * (float)System.Math.Sin(0.5 * r),
axis.y * (float)System.Math.Sin(0.5 * r),
axis.z * (float)System.Math.Sin(0.5 * r),
(float)System.Math.Cos(0.5 * r)
);
}
string CreateMeshEntity(Vector position, Quat orientation, Vector scale, MeshResource mesh)
{
Entity entity = new Entity();
entity["location"]["position"].Suggest(position);
entity["location"]["orientation"].Suggest(orientation);
entity["mesh"]["scale"].Suggest(scale);
entity["mesh"]["uri"].Suggest(mesh.uri);
entity["mesh"]["visible"].Suggest(mesh.visible);
World.Instance.Add(entity);
return entity.Guid.ToString ();
}
private void Update(string guid, Vector velocity, AxisAngle rotVelocity, int timestamp)
{
var entity = World.Instance.FindEntity(guid);
entity["motion"]["velocity"].Suggest(velocity);
entity["motion"]["rotVelocity"].Suggest(rotVelocity);
// We currently ignore timestamp, but may it in the future to implement dead reckoning.
}
private void setFrameQuaternion(string jointName, string x, string y, string z)
{
Quat q = new Quat(0, 0, 0, 1);
Vector xAxis = new Vector(1, 0, 0);
Vector yAxis = new Vector(0, 1, 0);
Vector zAxis = new Vector(0, 0, 1);
Vector rotData = new Vector(0, 0, 0);
double angle1 = System.Math.PI * double.Parse(x, CultureInfo.InvariantCulture) / 180.0;
double angle2 = System.Math.PI * double.Parse(y, CultureInfo.InvariantCulture) / 180.0;
double angle3 = System.Math.PI * double.Parse(z, CultureInfo.InvariantCulture) / 180.0;
if (Joints[jointName].channelOrder.Contains("XrotationYrotationZrotation")) //XYZ
{
q = FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle1);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle2));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle3));
rotData = new Vector((float)angle1, (float)angle2, (float)angle3);
}
else if (Joints[jointName].channelOrder.Contains("XrotationZrotationYrotation")) //XZY
{
q = FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle1);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle2));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle3));
rotData = new Vector((float)angle1, (float)angle3, (float)angle2);
}
else if (Joints[jointName].channelOrder.Contains("YrotationXrotationZrotation")) //YXZ
{
q = FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle1);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle2));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle3));
rotData = new Vector((float)angle2, (float)angle1, (float)angle3);
}
else if (Joints[jointName].channelOrder.Contains("YrotationZrotationXrotation")) //YZX
{
q = FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle1);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle2));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle3));
rotData = new Vector((float)angle2, (float)angle3, (float)angle1);
}
else if (Joints[jointName].channelOrder.Contains("ZrotationYrotationXrotation")) //ZYX
{
q = FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle1);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle2));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle3));
rotData = new Vector((float)angle3, (float)angle2, (float)angle1);
}
else if (Joints[jointName].channelOrder.Contains("ZrotationXrotationYrotation")) //ZXY
{
q = FIVES.Math.QuaternionFromAxisAngle(zAxis, (float)angle1);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, (float)angle2));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, (float)angle3));
rotData = new Vector((float)angle3, (float)angle1, (float)angle2);
}
//Console.WriteLine("quaternion: " + q.x + "," + q.y + "," + q.z + "," + q.w);
//Console.ReadLine();
Joints[jointName].quaternionData.Add(q);
Joints[jointName].rotationData.Add(rotData);
}
public static Vector ScaleVector(Vector vector, float scalar)
{
return new Vector(
vector.x * scalar,
vector.y * scalar,
vector.z * scalar
);
}
public static float VectorLength(Vector vector)
{
return (float)System.Math.Sqrt(ScalarProduct(vector, vector));
}
void SetLeftRightMotion(Connection connection, float amount)
{
var avatarEntity = GetAvatarEntityByConnection(connection);
Vector oldVelocity = (Vector)avatarEntity["motion"]["velocity"].Value;
Vector newVelocity = new Vector(oldVelocity.x, oldVelocity.y, amount);
avatarEntity["motion"]["velocity"].Suggest(newVelocity);
}
private void readAndProcessMotionData(string[] motionData)
{
//It is special for human body skeleton： normally only root joint has 6 channels, other joints only have 3 channels
int count = 0;
//Console.WriteLine(motionData.Length);
foreach (KeyValuePair<string, BVHJointData> joint in Joints)
{
if (joint.Value.isRoot == 1)
{
Vector trans = new Vector(float.Parse(motionData[count], CultureInfo.InvariantCulture), float.Parse(motionData[count+1], CultureInfo.InvariantCulture), float.Parse(motionData[count+2], CultureInfo.InvariantCulture));
Vector offset = joint.Value.offsetData;
//TODO: I use default translation channel order(XYZ), but it is in fact wrong
Joints[joint.Key].translationData.Add(FIVES.Math.AddVectors(trans, offset));
count += 3;
this.setFrameQuaternion(joint.Key, motionData[count], motionData[count + 1], motionData[count + 2]);
count += 3;
}
else if (joint.Value.isEndSite == 1)
{
//Ignore it..
}
else // isRoot ==0, isEndSite == 0
{
Vector trans = new Vector(0, 0, 0);
Vector offset = joint.Value.offsetData;
Joints[joint.Key].translationData.Add(FIVES.Math.AddVectors(trans, offset));
this.setFrameQuaternion(joint.Key, motionData[count], motionData[count + 1], motionData[count + 2]);
count += 3;
}
}
//Console.WriteLine("Count: " + count);
}
void SetAvatarSpinAroundAxis(Connection connection, Vector axis, float angle)
{
var avatarEntity = GetAvatarEntityByConnection(connection);
avatarEntity["motion"]["rotVelocity"].Suggest(new AxisAngle(axis, angle));
}
/// <summary>
/// Rotates a vector v around an axis n by an angle a by the formula
/// result = x*cos a + n(n.v)(1-cos a) + (v x n)sin a
/// </summary>
/// <param name="vector"></param>
/// <param name="axis"></param>
/// <param name="angle"></param>
/// <returns></returns>
public static Vector RotateVectorByAxisAngle(Vector vector, Vector axis, float angle)
{
Vector result = new Vector();
float cosAngle = (float)System.Math.Cos(angle);
float sinAngle = (float)System.Math.Sin(angle);
Vector scaledVector = ScaleVector(vector, cosAngle); // v*cos a
float axisFactor = ScalarProduct(vector, axis)*(1 - cosAngle); // (n.v)(1 - cos a)
Vector scaledAxis = ScaleVector(axis, axisFactor); // n(n.v)(1-cos a)
Vector scaledCross = ScaleVector(CrossProduct(vector, axis), sinAngle); // (v x n)sin a
result = AddVectors(AddVectors(scaledAxis , scaledVector), scaledCross);
return result;
}
/// <summary>
/// Worker Thread function that periodically performs the motion. Ends, when velocity of entity is 0
/// </summary>
/// <param name="updatedEntity">Entity for which motion is updated</param>
internal void UpdateMotion(Entity updatedEntity)
{
Vector localVelocity = GetVelocityInWorldSpace(updatedEntity);
Vector oldPosition = (Vector)updatedEntity["location"]["position"].Value;
Vector newPosition = new Vector(
oldPosition.x + localVelocity.x,
oldPosition.y + localVelocity.y,
oldPosition.z + localVelocity.z
);
updatedEntity["location"]["position"].Suggest(newPosition);
}
/// <summary>
/// Checks if position of entity has changed and sets y attribute to ground level if y and ground level
/// are different
/// </summary>
/// <param name="accumulatedTransforms">Accumulated transformation that happened in the service chain</param>
/// <returns>Accumulated changes with adaptions added by AvatarCollison</returns>
internal AccumulatedAttributeTransform Transform(AccumulatedAttributeTransform accumulatedTransforms)
{
if (!accumulatedTransforms.Entity.ContainsComponent("avatar"))
return accumulatedTransforms;
Vector entityPosition = (Vector)accumulatedTransforms.CurrentAttributeValue("location", "position");
Vector adaptedPosition = new Vector (entityPosition.x,
(float)accumulatedTransforms.Entity["avatarCollision"]["groundLevel"].Value,
entityPosition.z);
accumulatedTransforms.AddAttributeTransformation("location", "position", adaptedPosition);
return accumulatedTransforms;
}
public AxisAngle(Vector axis, float angle)
: this()
{
this.axis = axis;
this.angle = angle;
}
/// <summary>
/// Changes the appearance of the avatar.
/// </summary>
/// <param name="connection">Client connection</param>
/// <param name="meshURI">New mesh URI.</param>
/// <param name="scale">New scale.</param>
void ChangeAppearance(Connection connection, string meshURI, Vector scale)
{
var avatarEntity = GetAvatarEntityByConnection(connection);
avatarEntity["mesh"]["uri"].Suggest(meshURI);
avatarEntity["mesh"]["scale"].Suggest(scale);
}
private void UpdatePosition(string guid, Vector position, int timestamp)
{
var entity = World.Instance.FindEntity(guid);
entity["location"]["position"].Suggest(position);
// We currently ignore timestamp, but may it in the future to implement dead reckoning.
}
void SetForwardBackwardMotion(Connection connection, float amount)
{
var avatarEntity = GetAvatarEntityByConnection(connection);
Vector oldVelocity = (Vector)avatarEntity["motion"]["velocity"].Value;
Vector newVelocity = new Vector(amount, oldVelocity.y, oldVelocity.z);
avatarEntity["motion"]["velocity"].Suggest(newVelocity);
}
public static float ScalarProduct(Vector vector1, Vector vector2)
{
return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
}
void StartAvatarMotionInDirection(Connection connection, Vector velocity)
{
var avatarEntity = GetAvatarEntityByConnection(connection);
avatarEntity["motion"]["velocity"].Suggest(velocity);
}
private void applyAdditionalJointControl(string jointName, int index, float x, float y, float z)
{
Quat q = new Quat(0, 0, 0, 1);
Vector xAxis = new Vector(1, 0, 0);
Vector yAxis = new Vector(0, 1, 0);
Vector zAxis = new Vector(0, 0, 1);
if (Joints[jointName].channelOrder.Contains("XrotationYrotationZrotation")) //XYZ
{
q = FIVES.Math.QuaternionFromAxisAngle(xAxis, x);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
}
else if (Joints[jointName].channelOrder.Contains("XrotationZrotationYrotation")) //XZY
{
q = FIVES.Math.QuaternionFromAxisAngle(xAxis, x);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
}
else if (Joints[jointName].channelOrder.Contains("YrotationXrotationZrotation")) //YXZ
{
q = FIVES.Math.QuaternionFromAxisAngle(yAxis, y);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
}
else if (Joints[jointName].channelOrder.Contains("YrotationZrotationXrotation")) //YZX
{
q = FIVES.Math.QuaternionFromAxisAngle(yAxis, y);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(zAxis, z));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
}
else if (Joints[jointName].channelOrder.Contains("ZrotationYrotationXrotation")) //ZYX
{
q = FIVES.Math.QuaternionFromAxisAngle(zAxis, z);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
}
else if (Joints[jointName].channelOrder.Contains("ZrotationXrotationYrotation")) //ZXY
{
q = FIVES.Math.QuaternionFromAxisAngle(zAxis, z);
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(xAxis, x));
q = FIVES.Math.MultiplyQuaternions(q, FIVES.Math.QuaternionFromAxisAngle(yAxis, y));
}
Joints[jointName].quaternionData[index] = q;
}
