/// <summary>
/// Function taken from:
/// https://gist.github.com/aeroson/043001ca12fe29ee911e
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
public static MVector3 ToEulerRad(this MQuaternion rotation)
{
double sqw = rotation.W * rotation.W;
double sqx = rotation.X * rotation.X;
double sqy = rotation.Y * rotation.Y;
double sqz = rotation.Z * rotation.Z;
double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
double test = rotation.X * rotation.W - rotation.Y * rotation.Z;
MVector3 v = new MVector3();
if (test > 0.4995f * unit)
{ // singularity at north pole
v.Y = 2f * Math.Atan2(rotation.Y, rotation.X);
v.X = Math.PI / 2;
v.Z = 0;
return(NormalizeAngles(v.Multiply(Rad2Deg)));
}
if (test < -0.4995f * unit)
{ // singularity at south pole
v.Y = -2f * Math.Atan2(rotation.Y, rotation.X);
v.X = -Math.PI / 2;
v.Z = 0;
return(NormalizeAngles(v.Multiply(Rad2Deg)));
}
MQuaternion q = new MQuaternion(rotation.W, rotation.Z, rotation.X, rotation.Y);
v.Y = Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (q.Z * q.Z + q.W * q.W)); // Yaw
v.X = Math.Asin(2f * (q.X * q.Z - q.W * q.Y)); // Pitch
v.Z = Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (q.Y * q.Y + q.Z * q.Z)); // Roll
return(NormalizeAngles(v.Multiply(Rad2Deg)));
}
/// <summary>
/// Implementation based on https://gamedevelopment.tutsplus.com/tutorials/understanding-steering-behaviors-collision-avoidance--gamedev-7777
/// </summary>
/// <param name="position"></param>
/// <param name="velocity"></param>
private MVector3 ComputCollisionAvoidance(MVector3 position, MVector3 velocity)
{
MVector3 normalizedVelocity = velocity.Normalize();
float MAX_SEE_AHEAD = 0.4f;
float MAX_AVOID_FORCE = 5f;
//ahead = position + normalize(velocity) * MAX_SEE_AHEAD
MVector3 ahead = position.Add(normalizedVelocity.Multiply(MAX_SEE_AHEAD));
MVector3 ahead2 = position.Add(normalizedVelocity.Multiply(MAX_SEE_AHEAD * 0.5f));
///The obstacles describing the body
List <Obstacle> obstacles = new List <Obstacle>();
MVector3 pelvisPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
MVector3 headPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.HeadJoint);
obstacles.Add(new Obstacle()
{
Center = pelvisPosition,
Radius = 0.45f
});
obstacles.Add(new Obstacle()
{
Center = headPosition,
Radius = 0.3f
});
Obstacle mostThreatening = findMostThreateningObstacle(position, ahead, ahead2, obstacles);
MVector3 avoidance = new MVector3(0, 0, 0);
if (mostThreatening != null)
{
avoidance.X = ahead.X - mostThreatening.Center.X;
avoidance.Y = ahead.Y - mostThreatening.Center.Y;
avoidance.Z = ahead.Z - mostThreatening.Center.Z;
avoidance = avoidance.Normalize();
avoidance = avoidance.Multiply(MAX_AVOID_FORCE);
}
else
{
avoidance = avoidance.Multiply(0);
}
return(avoidance);
}
/// <summary>
/// Performs a local motion planning to estimate the next pose
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a resulting transform
MTransform result = new MTransform();
//Estimate the delta
MVector3 deltaPosition = targetPosition.Subtract(currentPosition);
//Estimate the meximum allowed delta
double maxTranslationDelta = velocity * time.TotalSeconds;
//Limit the maximum
if (deltaPosition.Magnitude() >= maxTranslationDelta)
{
deltaPosition = deltaPosition.Normalize();
deltaPosition = deltaPosition.Multiply(maxTranslationDelta);
}
float angularVelocityReach = 100f;
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxAngle = angularVelocityReach * time.TotalSeconds;
//Estimate the blendweihgt for the oreitnation blending
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(deltaPosition);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Performs the actual motion planning
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(float velocity, float angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
MTransform result = new MTransform();
MVector3 delta = targetPosition.Subtract(currentPosition);
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxTranslationDelta = velocity * time.TotalSeconds;
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//To do consider self collision
double maxAngle = angularVelocity * time.TotalSeconds;
if (angle < maxAngle)
{
angle = maxAngle;
}
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(delta);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Computes the new (desired) hand position considering the offset of the collider (to avoid self-collisions)
/// </summary>
/// <param name="targetHandPosition"></param>
/// <param name="currentPosture"></param>
/// <returns></returns>
private MVector3 ComputeNewPositionWithOffset(MVector3 targetHandPosition, MAvatarPostureValues currentPosture)
{
//Optionally ensure that the object does not intersect the avatar
MCollider collider = this.SceneAccess.GetColliderById(this.objectTransform.ID);
//Determine the offset based on the respective collider
float offset = 0;
if (collider.SphereColliderProperties != null)
{
offset = (float)collider.SphereColliderProperties.Radius;
}
if (collider.BoxColliderProperties != null)
{
offset = (float)collider.BoxColliderProperties.Size.Magnitude();
}
if (collider.CapsuleColliderProperties != null)
{
offset = Math.Max((float)collider.CapsuleColliderProperties.Height, (float)collider.CapsuleColliderProperties.Radius);
}
//The offset could be also dynamically determined (using the mesh intersection distance or using Physics Compute Pentration in unity)
this.SkeletonAccess.SetChannelData(currentPosture);
//Get the shoulder positions
MVector3 leftShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.LeftShoulder);
MVector3 rightShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.RightShoulder);
//Compute the direction vector pointing from the avatar towards the respective hand
MVector3 dir = new MVector3(0, 0, 0);
switch (this.handJoint)
{
case MJointType.LeftWrist:
dir = leftShoulderPosition.Subtract(rightShoulderPosition).Normalize();
break;
case MJointType.RightWrist:
dir = rightShoulderPosition.Subtract(leftShoulderPosition).Normalize();
break;
}
//Add an offset on top of the position
return(targetHandPosition.Add(dir.Multiply(offset)));
}
/// <summary>
/// Performs local motion planning to reach the defined point
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the vector to reach the goal
MVector3 delta = targetPosition.Subtract(currentPosition);
float distance = delta.Magnitude();
//Determine the angular distance
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Compute the translation weight
float translationWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Compute the rotation weight
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
//Limit the translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//Compute the new position
result.Position = currentPosition.Add(delta);
if (angularVelocity == 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, translationWeight);
}
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
return(result);
}
/// <summary>
/// Rotates the current transform around the specific point and axis
/// </summary>
/// <param name="center">The rotation center</param>
/// <param name="axis">The rotation axis</param>
/// <param name="angle">The angle to rotate</param>
private static MTransform RotateAround(MTransform transform, MVector3 center, MVector3 axis, float angle)
{
MTransform res = new MTransform()
{
ID = System.Guid.NewGuid().ToString()
};
MVector3 pos = transform.Position;
MQuaternion rot = MQuaternionExtensions.FromEuler(axis.Multiply(angle)); // get the desired rotation
MVector3 dir = pos.Subtract(center); // find current direction relative to center
dir = rot.Multiply(dir); // rotate the direction
res.Position = center.Add(dir); // define new position
MQuaternion myRot = transform.Rotation;
res.Rotation = transform.Rotation.Multiply(MQuaternionExtensions.Inverse(myRot).Multiply(rot).Multiply(myRot));
return(res);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Reset the properties
this.UseCarryIK = false;
this.bothHandedCarry = false;
this.bothHandedState = CarryState.None;
this.simulationState = simulationState;
//Create a list which contains the hands which should be considered for carrying
List <HandContainer> toPlan = new List <HandContainer>();
if (instruction.Properties == null)
{
throw new Exception($"{this.Name}: No properties defined!");
}
//Extract the hand information
if (instruction.Properties.ContainsKey("Hand"))
{
switch (instruction.Properties["Hand"])
{
case "Left":
toPlan.Add(this.SetupHand(HandType.Left, instruction));
this.bothHandedCarry = false;
break;
case "Right":
toPlan.Add(this.SetupHand(HandType.Right, instruction));
this.bothHandedCarry = false;
break;
case "Both":
//Set flag for both handed carry
this.bothHandedCarry = true;
toPlan.Add(this.SetupHand(HandType.Left, instruction));
toPlan.Add(this.SetupHand(HandType.Right, instruction));
break;
}
}
else
{
toPlan.Add(this.SetupHand(HandType.Right, instruction));
}
//Use the carry target if defined
if (!instruction.Properties.GetValue(out this.CarryTargetName, "CarryTarget"))
{
this.CarryTargetName = null;
}
//Use the carry target if defined
if (!instruction.Properties.GetValue(out this.UseCarryIK, "UseCarryIK"))
{
UseCarryIK = false;
}
//Use carry distance if defined
if (!instruction.Properties.GetValue(out this.carryDistanceBothHanded, "CarryDistance"))
{
carryDistanceBothHanded = 0.65f;
}
//Use carry distance if defined
if (!instruction.Properties.GetValue(out this.carryHeightBothHanded, "CarryHeight"))
{
carryHeightBothHanded = 0.2f;
}
//Use carry distance if defined
if (!instruction.Properties.GetValue(out this.positionObjectVelocity, "Velocity"))
{
this.positionObjectVelocity = 1.0f;
}
//Compute and plan the relevant aspects of each hand
foreach (HandContainer hand in toPlan)
{
//Get the (initial) hand transform
MTransform handTransform = this.GetTransform(simulationState.Initial, hand.Type);
//Get the current transform of the scene object
MTransform sceneObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["TargetID"]);
//Get the hand pose
try
{
hand.HandPose = GetTransform(simulationState.Initial, hand.Type);
}
catch (Exception e)
{
Console.WriteLine("Problem estimating hand pose: " + e.Message + e.StackTrace);
}
//Compute the relative transform of the hand (hand relative to object)
hand.HandOffset = new MTransform("", sceneObjectTransform.InverseTransformPoint(handTransform.Position), sceneObjectTransform.InverseTransformRotation(handTransform.Rotation));
//Compute the inverse offset (object relative to hand)
hand.ObjectOffset = new MTransform("", handTransform.InverseTransformPoint(sceneObjectTransform.Position), handTransform.InverseTransformRotation(sceneObjectTransform.Rotation));
//Set state to positioning
hand.State = CarryState.Positioning;
}
//Do additional computations for both handed carry
if (bothHandedCarry)
{
//Set the state to positioning
this.bothHandedState = CarryState.Positioning;
//Assign the instruction
this.instruction = instruction;
//Get the current object transorm
MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(this.instruction.Properties["TargetID"]);
//Get the current root transform -> to do
MTransform rootTransform = GetTransform(this.simulationState.Initial, MJointType.PelvisCentre);
//Compute the relative object transform
this.relativeObjectRotation = rootTransform.InverseTransformRotation(currentObjectTransform.Rotation);
this.relativeObjectPosition = rootTransform.InverseTransformPoint(currentObjectTransform.Position);
//Manually specify a carry target
if (this.CarryTargetName == null || this.CarryTargetName.Length == 0)
{
MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
MVector3 forward = GetRootForwad(this.simulationState.Initial);
//Determine the ref transform rotation just consider the y axis rotation
refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));
//Compute the delta
//MVector3 delta = currentObjectTransform.Position.Subtract(refTransform.Position);
//MVector3 direction = new MVector3(delta.X, 0, delta.Z).Normalize();
//The carry position i
MVector3 carryPosition = refTransform.Position.Add(forward.Multiply(this.carryDistanceBothHanded)).Add(new MVector3(0, carryHeightBothHanded, 0f));
//Forwad + offset
this.internalCarryTransform = new MTransform("CarryTarget", refTransform.InverseTransformPoint(carryPosition), refTransform.InverseTransformRotation(currentObjectTransform.Rotation));
}
}
return(new MBoolResponse(true));
}
/// <summary>
/// Method performs a local motion planning and tries to reach the specified goal position and rotation using the given velocity,angular velocity and time.
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation, bool collisionAvoidance)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the delta
MVector3 delta = targetPosition.Subtract(currentPosition);
//Determine the current delta angle
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle in the current frame
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Estimate the blend weight for the rotation and position
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
float positionWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
if (collisionAvoidance)
{
MVector3 collisionAvoidanceForce = this.ComputCollisionAvoidance(currentPosition, delta);
//if (collisionAvoidanceForce.Magnitude() > 0)
// MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Collision avoidance force: " + collisionAvoidanceForce.Magnitude());
//Add the collision avoidance force on top
delta = delta.Add(collisionAvoidanceForce);
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
}
//Compute the new position
result.Position = currentPosition.Add(delta);
//Compute the new rotation by interpolating towards the target rotation
if (angularVelocity > 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
//Use the rotation weight
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, positionWeight);
}
//Return the simulation result
return(result);
}
/// <summary>
/// Default do step routine
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Set the channel data to reflect to current posture
SkeletonAccess.SetChannelData(simulationState.Current);
//Set the default rotation of the head and neck joints
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint, initialHeadRotation);
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint, initialNeckRotation);
//Create a transform representing the current head location
MTransform currentTransform = new MTransform()
{
ID = "",
Position = SkeletonAccess.GetGlobalJointPosition(AvatarDescription.AvatarID, MJointType.HeadJoint),
Rotation = SkeletonAccess.GetGlobalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint)
};
//Create a transform representing the parent location (neck)
MTransform parentTransform = new MTransform()
{
ID = "",
Position = SkeletonAccess.GetGlobalJointPosition(AvatarDescription.AvatarID, MJointType.C4C5Joint),
Rotation = SkeletonAccess.GetGlobalJointRotation(AvatarDescription.AvatarID, MJointType.C4C5Joint)
};
//The current head forward vector
MVector3 currentHeadForward = new MVector3(-1, 0, 0);
//Compute the local position of the desired object (relative to the neck)
MVector3 localPosition = parentTransform.InverseTransformPoint(this.gazeTarget.Position);
//Get the xz distance in local space
float distance = new MVector3(localPosition.X, 0, localPosition.Z).Magnitude();
float height = (float)localPosition.Y;
//Compute the current angle
float currentAngle = (float)(Math.Atan(height / distance) * 180 / Math.PI);
//Limit if below lower limit
if (currentAngle < lowerLimit)
{
localPosition.Y = Math.Tan(lowerLimit * Math.PI / 180) * distance;
}
//Limit if above upper angle limit
if (currentAngle > upperLimit)
{
localPosition.Y = Math.Tan(upperLimit * Math.PI / 180) * distance;
}
float maxYAngle = 80f;
//Limit xz position
float yAngle = (float)MVector3Extensions.Angle(currentHeadForward, new MVector3(localPosition.X, 0, localPosition.Z));
if (yAngle > maxYAngle)
{
//The interpolated direction
MVector3 interpolatedDirection = MVector3Extensions.Lerp(currentHeadForward, new MVector3(localPosition.X, 0, localPosition.Z).Normalize(), (maxYAngle / yAngle)).Normalize();
//Perform correction
MVector3 newLocalPositionsXZ = interpolatedDirection.Multiply(distance);
localPosition.X = newLocalPositionsXZ.X;
localPosition.Z = newLocalPositionsXZ.Z;
}
//Compute the desired and current facing direction
MVector3 desiredHeadForward = localPosition.Normalize();
//Estimate the rotation that is required to rotate from the current head direction towards the desired one
MQuaternion deltaRotation = FromToRotation(currentHeadForward, new MVector3(desiredHeadForward.X, -desiredHeadForward.Y, desiredHeadForward.Z));
//Gather the current location rotation
MQuaternion currentLocalRotation = SkeletonAccess.GetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint);
//Update the local joint rotation to adjust the facing direction to the desired values
SkeletonAccess.SetLocalJointRotation(AvatarDescription.AvatarID, MJointType.HeadJoint, currentLocalRotation.Multiply(deltaRotation));
//Set the updated postures
result.Posture = SkeletonAccess.RecomputeCurrentPostureValues(AvatarDescription.AvatarID);
//Return the simulation results
return(result);
}
