/// <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); }