/// <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)));
}
public MVector3 RetargetPositionToIS(MVector3 globalPos, MQuaternion globalRot)
{
MVector3 pos = globalPos.Add(globalRot.Multiply(this.targetOffset));
this.globalPos = pos;
return(pos);
}
//public void draw(MVector3 pos, MRectangle srcRect, MVector2 scale,
// MVector2 translate, MVector3 drawCenter, float angle, MVector4 color)
public static void draw(int textureID, int pos, int srcRect, int scale,
int translate, int drawCenter, float angle, int color)
{
//m_graphicHandlerRef.getSpriteBatch().Draw(m_texture, m_graphicHandlerRef.getGameInstance().Window.ClientBounds, Color.White);
Rectangle? rect;
MRectangle trueRect = MRectangle.getFromStorage(srcRect);
if (trueRect != null)
{
rect = trueRect.getRawData();
}
else
{
rect = null;
}
Vector2 scaleParam = MVector2.getFromStorage(scale).getRawData();
SpriteEffects sEffect = generateSpriteEffect(ref scaleParam);
Vector2 translateParam = MVector3.getFromStorage(pos).getRawAxistedVec2()
+ MVector2.getFromStorage(translate).getRawData();
m_graphicHandlerRef.getSpriteBatch()
.Draw(
MTexture.getFromStorage(textureID).m_texture,
translateParam,
rect,
MVector4.getFromStorage(color).getColor(),
angle,
MVector3.getFromStorage(drawCenter).getRawVec2(),
scaleParam,
sEffect,
0.0f);
}
private static MVector3 NormalizeAngles(MVector3 angles)
{
angles.X = NormalizeAngle(angles.X);
angles.Y = NormalizeAngle(angles.Y);
angles.Z = NormalizeAngle(angles.Z);
return(angles);
}
public static void drawText(String content, int posID, int colorID, int dockType)
{
Vector2 stringSize = m_spriteFont.MeasureString(content);
Vector2 position = MVector3.getFromStorage(posID).getRawVec2();
Vector2 origin;
switch (dockType)
{
case DT_LEFT:
origin = new Vector2(0.0f, 0.0f);
break;
case DT_CENTER:
position.X -= stringSize.X / 2.0f;
origin = new Vector2(stringSize.X / 2.0f, 0.0f);
break;
case DT_RIGHT:
position.X -= stringSize.X;
origin = new Vector2(stringSize.X, 0.0f);
break;
default:
break;
}
m_graphicHandlerRef.getSpriteBatch().DrawString(m_spriteFont, content, position, MVector4.getFromStorage(colorID).getColor());
}
/// <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 rotation to rotate from one vector to the other
/// </summary>
/// <param name="from">The start direction</param>
/// <param name="to">The desired direction</param>
/// <returns></returns>
private static MQuaternion FromToRotation(MVector3 from, MVector3 to)
{
//Normalize both vectors
from = from.Normalize();
to = to.Normalize();
//Estimate the rotation axis
MVector3 axis = MVector3Extensions.Cross(from, to).Normalize();
//Compute the phi angle
double phi = Math.Acos(MVector3Extensions.Dot(from, to)) / (from.Magnitude() * to.Magnitude());
//Create a new quaternion representing the rotation
MQuaternion result = new MQuaternion()
{
X = Math.Sin(phi / 2) * axis.X,
Y = Math.Sin(phi / 2) * axis.Y,
Z = Math.Sin(phi / 2) * axis.Z,
W = Math.Cos(phi / 2)
};
//Perform is nan check and return identity quaternion
if (double.IsNaN(result.W) || double.IsNaN(result.X) || double.IsNaN(result.Y) || double.IsNaN(result.Z))
{
result = new MQuaternion(0, 0, 0, 1);
}
//Return the estimated rotation
return(result);
}
// the following functions can be used to parse MAvatarDescriptions from a bvh-like file. These functions
// are currently not maintained and might be outdated.
#region parsing of MAvatarPosture files
private static MJoint ParseJoint(string[] mos, ref int line_counter, ref List <MJoint> mjointList)
{
// parse lines for current joint
// Todo: Improve parser to consider empty lines and comments
string name = mos[line_counter].Split(' ')[1];
float[] off = parseFloatParameter(mos[line_counter + 2].Split(' '), 3);
MVector3 offset = new MVector3(off[0], off[1], off[2]);
float[] quat = parseFloatParameter(mos[line_counter + 3].Split(' '), 4);
MQuaternion rotation = new MQuaternion(quat[1], quat[2], quat[3], quat[0]);
string[] channels = mos[line_counter + 4].Replace("CHANNELS", "").Split(' ');
List <MChannel> mchannels = MapChannels(channels);
MJoint mjoint = new MJoint(name, MJointTypeMap[name], offset, rotation);
mjoint.Channels = mchannels;
mjointList.Add(mjoint);
line_counter += 5;
while (!mos[line_counter].Contains("}"))
{
MJoint child = ParseJoint(mos, ref line_counter, ref mjointList);
child.Parent = mjoint.ID;
}
line_counter += 1;
return(mjoint);
}
/// <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>
/// Determines whether the given translation constraint is fulfilled
/// </summary>
/// <param name="desiredPosition"></param>
/// <param name="currentPosition"></param>
/// <param name="translationConstraint"></param>
/// <returns></returns>
private bool IsFulfilled(MVector3 desiredPosition, MVector3 currentPosition, MTranslationConstraint translationConstraint)
{
//By default return true -> It is assumed that interval is [-inv,+inv]
if (translationConstraint == null)
{
return(true);
}
switch (translationConstraint.Type)
{
case MTranslationConstraintType.BOX:
//Determine the global min and max coordinate
MVector3 min = new MVector3(translationConstraint.Limits.X.Min, translationConstraint.Limits.Y.Min, translationConstraint.Limits.Z.Min).Add(currentPosition);
MVector3 max = new MVector3(translationConstraint.Limits.X.Max, translationConstraint.Limits.Y.Max, translationConstraint.Limits.Z.Max).Add(currentPosition);
//Check if within bounding box
return(currentPosition.X >= min.X && currentPosition.Y >= min.Y && currentPosition.Z >= min.Z && currentPosition.X <= max.X && currentPosition.Y <= max.Y && currentPosition.Z <= max.Z);
case MTranslationConstraintType.ELLIPSOID:
double xDist = Math.Abs(currentPosition.X - desiredPosition.X);
double yDist = Math.Abs(currentPosition.Y - desiredPosition.Y);
double zDist = Math.Abs(currentPosition.Z - desiredPosition.Z);
return(xDist <= translationConstraint.Limits.X() && yDist <= translationConstraint.Limits.Y() && zDist <= translationConstraint.Limits.Z());
}
//By default return true -> It is assumed that interval is [-inv,+inv]
return(true);
}
/// <summary>
/// Retargets the global posture to the intermediate skeleton
/// </summary>
/// <param name="globalTarget"></param>
/// <returns></returns>
public MAvatarPostureValues RetargetToIntermediate(MAvatarPosture globalTarget)
{
RJoint root = ((RJoint)this.skeleton.GetRoot(globalTarget.AvatarID));
bool rootFound = false;
foreach (MJoint j in globalTarget.Joints)
{
if (j.Type != MJointType.Undefined)
{
RJoint rj = ((RJoint)root.GetChild(j.Type));
rj.RetargetPositionToIS(j.Position, j.Rotation);
rj.RetargetRotationToIS(j.Rotation);
if (!rootFound)
{
rootFound = true;
if (j.Type == MJointType.Root)
{
}
else
{
MVector3 globalPos = rj.GetGlobalPosManually();
root.SetGlobalPosManually(new MVector3(globalPos.X, 0, globalPos.Z));
//rj.SetGlobalPosManually(new MVector3(0, globalPos.Y, 0));
}
}
}
}
root.RecomputeLocalTransformations();
MAvatarPostureValues ret = new MAvatarPostureValues(globalTarget.AvatarID, root.GetAvatarPostureValues());
return(ret);
}
/// <summary>
/// Applies this transform to the other transform.
/// </summary>
/// <param name="transform"></param>
/// <param name="other"></param>
/// <returns></returns>
public static MTransform Multiply(this MTransform transform, MTransform other)
{
MQuaternion q = transform.Rotation.Multiply(other.Rotation);
MVector3 pos = other.Rotation.Multiply(transform.Position).Add(other.Position);
MTransform t = new MTransform(transform.ID, pos, q);
return(t);
}
/// <summary>
/// Returns the translation distance to move the hand from the initial state to the current state
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
private float GetHandDistance(HandType type)
{
MVector3 targetHandPosition = this.GetGlobalPosition(simulationState.Current, type);
MVector3 currentHandPosition = this.GetGlobalPosition(simulationState.Initial, type);
return(targetHandPosition.Subtract(currentHandPosition).Magnitude());
}
private static MJoint NewMJoint(string id, MJointType type, MVector3 offset, MQuaternion rotation, string parentID, List <MChannel> channels)
{
MJoint j = new MJoint(id, type, offset, rotation);
j.Parent = parentID;
j.Channels = channels;
return(j);
}
/// <summary>
/// Implementation of the check prerequisites method which is used by the co-simulation to determine whether the MMU can be started
/// </summary>
/// <param name="instruction"></param>
/// <returns></returns>
public override MBoolResponse CheckPrerequisites(MInstruction instruction)
{
//Get the min distance parameter
if (instruction.Properties != null && instruction.Properties.ContainsKey("MinDistance"))
{
instruction.Properties.GetValue(out minReachDistance, "MinDistance");
}
else
{
minReachDistance = minDistanceDefault;
}
if (instruction.Properties.ContainsKey("TargetID"))
{
MSceneObject sceneObject = this.SceneAccess.GetSceneObjectByID(instruction.Properties["TargetID"]);
MAvatar avatar = this.SceneAccess.GetAvatarByID(this.AvatarDescription.AvatarID);
if (sceneObject != null && avatar != null)
{
this.SkeletonAccess.SetChannelData(avatar.PostureValues);
//Get the root position
MVector3 rootPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
rootPosition.Y = 0;
//Get the object position
MVector3 objectPosition = new MVector3(sceneObject.Transform.Position.X, 0, sceneObject.Transform.Position.Z);
//Compute the distance between root and object
float distance = rootPosition.Subtract(objectPosition).Magnitude();
//Check if below distance
if (distance < this.minReachDistance)
{
if (this.debug)
{
Logger.Log(Log_level.L_DEBUG, $"Check prerequisites of reach successfull! Distance: {distance}/{minReachDistance}");
}
return(new MBoolResponse(true));
}
else
{
if (this.debug)
{
Logger.Log(Log_level.L_DEBUG, $"Check prerequisites of reach failed! Distance: {distance}/{minReachDistance}");
}
return(new MBoolResponse(false));
}
}
}
return(new MBoolResponse(true));
}
/// <summary>
/// Returns the forward vector of the root transform
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MVector3 GetGlobalDirection(MAvatarPostureValues posture, MVector3 localDireciton)
{
MTransform rootTransform = this.GetRootTransform(posture);
//Compute the forwad vector of the root transform
MVector3 rootForward = rootTransform.Rotation.Multiply(localDireciton);
rootForward.Y = 0;
rootForward = rootForward.Normalize();
return(rootForward);
}
private MVector3 GetFingerPosition(MAvatarPosture globalTarget, Dictionary <MJointType, string> joint_map, double z_shift)
{
MVector3 wristPos = getJointPosition(globalTarget, joint_map[this.parentJoint.GetMJoint().Type]);//this.parentJoint.GetGlobalPosition();
MVector3 thisPos = getJointPosition(globalTarget, joint_map[this.GetMJoint().Type]);
thisPos.Y = wristPos.Y; // adjust height
MVector3 newPos = new MTransform("tmp", wristPos, this.parentJoint.GetGlobalRotation()).InverseTransformPoint(thisPos);
newPos.Z -= z_shift;
return(newPos);
}
/// <summary>
/// Returns the forward vector of the root transform
/// </summary>
/// <param name="posture"></param>
/// <returns></returns>
private MVector3 GetRootForwad(MAvatarPostureValues posture)
{
MTransform rootTransform = this.GetRootTransform(posture);
//Compute the forwad vector of the root transform
MVector3 rootForward = rootTransform.Rotation.Multiply(new MVector3(0, 0, 1));
rootForward.Y = 0;
rootForward = rootForward.Normalize();
return(rootForward);
}
// The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees.
// If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper.
// The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction.
public static double SignedAngle(MVector3 from, MVector3 to, MVector3 axis)
{
double unsignedAngle = Angle(from, to);
double cross_x = from.Y * to.Z - from.Z * to.Y;
double cross_y = from.Z * to.X - from.X * to.Z;
double cross_z = from.X * to.Y - from.Y * to.X;
double sign = Math.Sign(axis.X * cross_x + axis.Y * cross_y + axis.Z * cross_z);
return(unsignedAngle * sign);
}
/// <summary>
/// Method is repsonsible for modeling the actual carry for both handed objects
/// </summary>
/// <param name="result"></param>
private void BothHandedCarry(ref MSimulationResult result)
{
//Create an empty transform representing the next object transform
MTransform nextObjectTransform = new MTransform();
//Update the desired object transform
if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
{
MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
nextObjectTransform.Position = targetTransform.Position;
nextObjectTransform.Rotation = targetTransform.Rotation;
}
else
{
MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
MVector3 forward = GetRootForwad(this.simulationState.Initial);
//Determine the ref transform rotation
refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));
nextObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
nextObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
}
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = instruction.Properties["TargetID"],
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
List <MIKProperty> ikProperties = new List <MIKProperty>();
//Update the hands
foreach (HandContainer hand in this.ActiveHands)
{
//Update the hands
MTransform nextHandPose = new MTransform("", nextObjectTransform.TransformPoint(hand.HandOffset.Position), nextObjectTransform.TransformRotation(hand.HandOffset.Rotation));
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
}
}
/// <summary>
/// Returns all scene objects within the specified range
/// </summary>
/// <param name="position"></param>
/// <param name="distance"></param>
/// <returns></returns>
public List <MSceneObject> GetSceneObjectsInRange(MVector3 position, double distance)
{
List <MSceneObject> result = new List <MSceneObject>();
foreach (MSceneObject sceneObject in this.GetSceneObjects())
{
if (EuclideanDistance(sceneObject.Transform.Position, position) <= distance)
{
result.Add(sceneObject);
}
}
return(result);
}
/// <summary>
/// Sets an endeffector constraint with a given local position and rotation and a parent
/// </summary>
/// <param name="type"></param>
/// <param name="position"></param>
/// <param name="rotation"></param>
/// <param name="parentID">The id of the parent</param>
public virtual void SetEndeffectorConstraint(MJointType type, MVector3 position, MQuaternion rotation, String id = null, String parentID = "")
{
//Create a new endeffector constriant using the MJointConstraint
this.SetEndeffectorConstraint(new MJointConstraint()
{
JointType = type,
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = parentID,
ParentToConstraint = new MTransform(Guid.NewGuid().ToString(), position, rotation)
},
}, id);
}
/// <summary>
/// Returns all colliders within the specified range (if available)
/// </summary>
/// <param name="position"></param>
/// <param name="range"></param>
/// <returns></returns>
public List <MCollider> GetCollidersInRange(MVector3 position, double range)
{
List <MCollider> result = new List <MCollider>();
foreach (MSceneObject sceneObject in this.GetSceneObjectsInRange(position, range))
{
if (sceneObject.Collider != null)
{
result.Add(sceneObject.Collider);
}
}
return(result);
}
/// <summary>
/// Will draw : Name & Health Bar
/// </summary>
/// <param name="context"></param>
private void DrawEntitiesOverHeadData(DeviceContext context)
{
_dynamicEntityNameRenderer.Begin(context, true);
_dynamicEntityEnergyBarRenderer.Begin(context, true);
foreach (VisualDynamicEntity dynamicEntity in _dynamicEntitiesDico.Values.Where(x => x.ModelInstance != null && x.ModelInstance.World != Matrix.Zero))
{
bool isMultiline = false;
string Name;
Vector3 textPosition = dynamicEntity.WorldPosition.ValueInterp.AsVector3();
textPosition.Y += (dynamicEntity.ModelInstance.State.BoundingBox.Maximum.Y / 16); //Place the text above the BoundingBox
ByteColor color = Color.White;
if (dynamicEntity.DynamicEntity is CharacterEntity)
{
Name = ((CharacterEntity)dynamicEntity.DynamicEntity).CharacterName;
//Is it the local player ?
if (IsLocalPlayer(dynamicEntity.DynamicEntity.DynamicId))
{
color = Color.Yellow;
}
else
{
Name += Environment.NewLine + "<" + dynamicEntity.DynamicEntity.Name + ">";
isMultiline = true;
}
}
else
{
Name = dynamicEntity.DynamicEntity.Name;
color = Color.WhiteSmoke;
}
var distance = MVector3.Distance(dynamicEntity.WorldPosition.ValueInterp, _camManager.ActiveCamera.WorldPosition.ValueInterp);
float scaling = Math.Min(0.040f, Math.Max(0.01f, 0.01f / 12 * (float)distance));
_dynamicEntityNameRenderer.Processor.DrawText(Name, ref textPosition, scaling, ref color, _camManager.ActiveCamera, MultiLineHandling: isMultiline);
//HBar rendering
if (!IsLocalPlayer(dynamicEntity.DynamicEntity.DynamicId) && dynamicEntity.DynamicEntity.Health.CurrentAsPercent < 1 && dynamicEntity.DynamicEntity.Health.CurrentAsPercent > 0)
{
var size = new Vector2((dynamicEntity.ModelInstance.State.BoundingBox.Maximum.X / 8) * dynamicEntity.DynamicEntity.Health.CurrentAsPercent, 0.1f);
Vector3 hbPosition = textPosition;
hbPosition.Y += 0.05f;
_dynamicEntityEnergyBarRenderer.Processor.Draw(ref hbPosition, ref size, ref hbColor, ref colorReceived);
}
}
_dynamicEntityNameRenderer.End(context);
_dynamicEntityEnergyBarRenderer.End(context);
}
public static double Angle(MVector3 from, MVector3 to)
{
// sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
double denominator = Math.Sqrt(from.SqrMagnitude() * to.SqrMagnitude());
if (denominator < kEpsilonNormalSqrt)
{
return(0F);
}
double dot = Clamp(Dot(from, to) / denominator, -1F, 1F);
return((Math.Acos(dot)) * 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>
/// Returns the avatars in range of the specified position
/// </summary>
/// <param name="position"></param>
/// <param name="distance"></param>
/// <returns></returns>
public List <MAvatar> GetAvatarsInRange(MVector3 position, double distance)
{
List <MAvatar> result = new List <MAvatar>();
foreach (MAvatar avatar in this.GetAvatars())
{
MVector3 avatarPosition = new MVector3(avatar.PostureValues.PostureData[0], avatar.PostureValues.PostureData[1], avatar.PostureValues.PostureData[2]);
if (avatarPosition.Subtract(position).Magnitude() <= distance)
{
result.Add(avatar);
}
}
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);
}
public static MSceneObject CreateSceneObject(string name, MVector3 position, MQuaternion rotation, string parent = null)
{
string id = Guid.NewGuid().ToString();
MSceneObject sceneObject = new MSceneObject()
{
ID = id,
Name = name,
Transform = new MTransform(id, position, rotation)
{
Parent = parent
},
Properties = new Dictionary <string, string>(),
};
return(sceneObject);
}
