/// <summary>
/// Sets the position and rotation of this point, relative to the provided parent.
/// </summary>
/// <param name="x">The relative x position</param>
/// <param name="z">The relative z position</param>
/// <param name="angle">The relative angle</param>
/// <param name="parent">The motion point this one will be relative to</param>
public void SetRelative(float x, float z, float angle, MotionPoint parent)
{
RelativeX = x;
RelativeZ = z;
RelativeAngle = angle;
Bake(parent.BakedMatrix);
}
/// <summary>
/// Creates a new motion point with the provided position and rotation, relative to the provided parent.
/// </summary>
/// <param name="x">The relative x position</param>
/// <param name="z">The relative z position</param>
/// <param name="angle">The relative angle</param>
/// <param name="parent">The motion point the created one will be relative to</param>
/// <returns>A new motion point</returns>
public static MotionPoint Relative(float x, float z, float angle, MotionPoint parent)
{
MotionPoint point = Identity;
point.SetRelative(x, z, angle, parent);
return(point);
}
public JObject ToJObject()
{
JObject jFile = new JObject();
jFile.Add("Version", SystemInfo.Version);
AddItemRecursive(jFile, rootFolder);
void AddItemRecursive(JObject jParent, MotionItemBase item)
{
JObject jItem = new JObject();
jParent.Add(item.IsRoot ? RootFolderName : item.Name, jItem);
jItem.Add("Type", item.Type.ToString());
if (item.Type == MotionItemType.Motion)
{
JObject jData = new JObject();
jItem.Add("Data", jData);
SaveMotion(jData, item as MotionItem);
}
else
{
JObject jItems = new JObject();
jItem.Add("Items", jItems);
SaveFolder(jItems, item as MotionFolderItem);
}
}
void SaveMotion(JObject jData, MotionItem motion)
{
JArray jPoints = new JArray();
jData.Add("Point", jPoints);
for (int pointI = 0; pointI < motion.pointList.Count; ++pointI)
{
JArray jPoint = new JArray();
jPoints.Add(jPoint);
MotionPoint point = motion.pointList[pointI];
jPoint.Add(point.MainPoint.ToString());
jPoint.Add(point.SubPoints[0].ToString());
jPoint.Add(point.SubPoints[1].ToString());
}
}
void SaveFolder(JObject jData, MotionFolderItem folder)
{
for (int i = 0; i < folder.childList.Count; ++i)
{
MotionItemBase childItem = folder.childList[i];
AddItemRecursive(jData, childItem);
}
}
return(jFile);
}
/// <summary>
/// Computes a new motion point by applying the provided relative motion to this point.
/// The returned point will loose any relation it had to a parent point.
/// </summary>
/// <param name="motion">The relative motion to apply</param>
/// <returns>A new motion point</returns>
public MotionPoint RelativeMove(Motion motion)
{
MotionPoint newPoint;
//Get the angle of the two points
float startAngle = Angle;
float endAngle = startAngle + motion.AngularVelocity;
if (motion.HasTranslation())
{
//Calculate the end position relative to the start point
Vector3 endPosition;
if (motion.HasRotation())
{
Vector3 localRotOrigin = Util.Rotate(motion.RotationOrigin, startAngle) + Position;
endPosition = Util.Rotate(Position - localRotOrigin, motion.AngularVelocity) + localRotOrigin;
}
else
{
endPosition = Util.Rotate(motion.LinearVelocity, startAngle) + Position;
}
newPoint = new MotionPoint(endPosition.x, endPosition.z, endAngle);
}
else
{
newPoint = new MotionPoint(Position.x, Position.z, endAngle);
}
return(newPoint);
}
/// <summary>
///
/// </summary>
/// <param name="parent"></param>
/// <param name="legList"></param>
private void FindChildFeet(Transform parent, List <LegData> legList)
{
foreach (Transform child in parent)
{
FootMotionRegion region = child.GetComponent <FootMotionRegion>();
if (region != null)
{
MotionPoint initialPoint = MotionPoint.FromTransform(_bodyTransform, region.transform, _rootPoint);
LegData stuff = new LegData
{
region = region,
point = initialPoint,
debugLastPosition = region.transform.position + new Vector3(0.0f, 0.0f, 0.0f),// stuff.channel.Height, stuff.channel.Offset);
gaitPoint = new Point(initialPoint.Position, initialPoint.Angle),
gaitError = new Point(Vector3.zero, 0.0f),
finalPoint = new Point(initialPoint.Position, initialPoint.Angle),
};
legList.Add(stuff);
}
else
{
//The child isn't anything so look at its children
FindChildFeet(child, legList);
}
}
}
/// <summary>
/// Create a motion point from a provided transform, that is relative to a base transform.
/// The returned MotionPoint will be local to a parent MotionPoint
/// </summary>
/// <param name="baseTrans"></param>
/// <param name="trans"></param>
/// <param name="parent"></param>
/// <returns></returns>
public static MotionPoint FromTransform(Transform baseTrans, Transform trans, MotionPoint parent)
{
Vector3 localPos; Quaternion localRot;
Util.CalcRelativeTo(baseTrans, trans, out localPos, out localRot);
return(Local(localPos.x, localPos.z, localRot.eulerAngles.y, parent));
}
public float GetMotionValue(float linearValue, int maxSample = DefaultMaxSample, float tolerance = DefaultMaxTolerance)
{
linearValue = Math.Max(Math.Min(linearValue, 1f), 0f);
int rightIndex = GetRightPointIndex(linearValue);
if (rightIndex == -1)
{
if (pointList.Count > 0)
{
//마지막 포인트를 벗어나면 마지막 좌표 반환
return(pointList[pointList.Count - 1].MainPoint.y);
}
else
{
//포인트가 하나이거나 없을 때 1 반환
return(1);
}
}
MotionPoint left = pointList[rightIndex - 1];
MotionPoint right = pointList[rightIndex];
return(PSpline.Bezier3_X2Y(linearValue, left.MainPoint, left.GetAbsoluteSubPoint(1), right.GetAbsoluteSubPoint(0), right.MainPoint, maxSample, tolerance));
}
/// <summary>
/// Converts this motion from being relative to one motion point to being relative to another
/// </summary>
/// <param name="currentPoint">The point this motion is relative to</param>
/// <param name="newPoint">The point the returned motion is to be relative to</param>
/// <returns>A new motion that is relative to the newPoint</returns>
public Motion Convert(MotionPoint currentPoint, MotionPoint newPoint)
{
Vector3 newLinVelocity, newRotOrigin;
if (HasRotation())
{
//Compute the new rotation origin
Vector3 localRotOrigin = Util.Rotate(RotationOrigin, currentPoint.Angle) + currentPoint.Position;
newRotOrigin = Util.Rotate(localRotOrigin - newPoint.Position, -newPoint.Angle);
//Calculate the new linear velocity
float newPathRadius = newRotOrigin.magnitude;
if (newPathRadius > 0.0f)
{
float arcLength = newPathRadius * AngularVelocity * Mathf.Deg2Rad;
newLinVelocity = Util.RotateMinus90(newRotOrigin).normalized *arcLength;
}
else
{
newLinVelocity = Vector3.zero;
}
}
else
{
//Calculate the new linear velocity and set the rotation origin to NaN
newLinVelocity = Util.Rotate(LinearVelocity, currentPoint.Angle - newPoint.Angle);
newRotOrigin = new Vector3(float.NaN, 0.0f, float.NaN);
}
return(new Motion(newLinVelocity, AngularVelocity, newRotOrigin));
}
/// <summary>
/// Computes a new motion point by applying the provided local motion to this point.
/// The returned point will loose any relation it had to a parent point.
/// </summary>
/// <param name="motion">The local motion to apply</param>
/// <returns>A new motion point</returns>
public MotionPoint LocalMove(Motion motion)
{
MotionPoint newPoint;
//Get the angle of the two points
float startAngle = Angle;
float endAngle = startAngle + motion.AngularVelocity;
if (motion.HasTranslation())
{
//Calculate the end position in local space
Vector3 endPosition;
if (motion.HasRotation())
{
endPosition = Util.Rotate(Position - motion.RotationOrigin, motion.AngularVelocity) + motion.RotationOrigin;
}
else
{
endPosition = motion.LinearVelocity + Position;
}
newPoint = new MotionPoint(endPosition.x, endPosition.z, endAngle);
}
else
{
newPoint = new MotionPoint(Position.x, Position.z, endAngle);
}
return(newPoint);
}
/// <summary>
/// Computes a new motion point by applying the provided relative motion to this point.
/// </summary>
/// <param name="motion">The relative motion to apply</param>
/// <param name="parent">The motion point the new one will internally be relative to</param>
/// <returns>A new motion point</returns>
public MotionPoint RelativeMove(Motion motion, MotionPoint parent)
{
MotionPoint newPoint = RelativeMove(motion);
newPoint.SetLocalTo(parent);
return(newPoint);
}
/// <summary>
///
/// </summary>
private void UpdateLegPoints()
{
foreach (LegData leg in _legList)
{
//Create a copy of the current leg's point with root point's motion applied to it
leg.point = MotionPoint.FromTransform(_bodyTransform, leg.region.transform, _rootPoint);
}
}
/// <summary>
/// Creates a copy of this point with the same position and rotation in local space,
/// but internally relative to the provided parent.
/// </summary>
/// <param name="newParent">The motion point that will be the new parent</param>
/// <returns>A new motion point</returns>
public MotionPoint LocalTo(MotionPoint newParent)
{
//Workaround for situation where zeros get passed in rather than the relative values
float x = Position.x;
float z = Position.z;
float a = Angle;
return(Local(x, z, a, newParent));
}
private static MotionPoint PointFromTransform(Transform baseTrans, Transform trans, float yAngle, out Quaternion angOut, MotionPoint pointParent)
{
Vector3 localPos; Quaternion localRot;
Util.CalcRelativeTo(baseTrans, trans, out localPos, out localRot);
localRot *= Quaternion.Euler(0.0f, yAngle, 0.0f);
angOut = localRot;
return(MotionPoint.Local(localPos.x, localPos.z, localRot.eulerAngles.y, pointParent));
}
public void DuplicateSelectedMotion()
{
if (!IsSelectedItemCopyable)
{
return;
}
MotionItemBase latestNewItem = null;
MotionFolderItem parentFolder = SelectedItemParent;
foreach (MotionItemBaseView refItem in MotionTreeView.SelectedItemSet)
{
if (refItem.Type == MotionItemType.Motion)
{
MotionItem refMotionItem = (MotionItem)refItem.Data;
//Create motion
MotionItem newItem = EditingFile.CreateMotionEmpty(parentFolder);
latestNewItem = newItem;
//Copy points
foreach (MotionPoint refPoint in refMotionItem.pointList)
{
MotionPoint point = new MotionPoint();
newItem.AddPoint(point);
point.SetMainPoint(refPoint.MainPoint);
for (int i = 0; i < refPoint.SubPoints.Length; ++i)
{
point.SetSubPoint(i, refPoint.SubPoints[i]);
}
}
((MotionItemView)DataToViewDict[newItem]).UpdatePreviewGraph();
//Set name
const string CopyPostfix = "_Copy";
string name = refItem.Data.Name;
for (; ;)
{
if (EditingFile.itemDict.ContainsKey(name))
{
name += CopyPostfix;
}
else
{
break;
}
}
newItem.SetName(name);
}
}
if (latestNewItem != null)
{
MotionTreeView.SelectedItemSet.SetSelectedItem(DataToViewDict[latestNewItem]);
}
}
/// <summary>
/// Creates a copy of this point that is relative to the provided parent point.
/// </summary>
/// <param name="newParent">The motion point that will be the new parent</param>
/// <returns>A new motion point</returns>
public MotionPoint RelativeTo(MotionPoint newParent)
{
//Workaround for situation where zeros get passed in rather than the relative values
float x = RelativeX;
float z = RelativeZ;
float a = RelativeAngle;
return(Relative(x, z, a, newParent));
//return Relative(RelativeX, RelativeZ, RelativeAngle, newParent);
}
public MotionPointView(MotionEditorContext editorContext, MotionPoint data)
{
InitializeComponent();
this.EditorContext = editorContext;
this.Data = data;
InitViews();
RegisterEvent();
}
/// <summary>
/// Sets the position and rotation of this point in local space,
/// but internally relative to the provided parent.
/// </summary>
/// <param name="x">The local x position</param>
/// <param name="z">The local z position</param>
/// <param name="angle">The local angle</param>
/// <param name="parent">The motion point this one will internally be relative to</param>
public void SetLocal(float x, float z, float angle, MotionPoint parent)
{
float parentAngle = parent.Angle;
Vector3 inverse = Util.Rotate(new Vector3(x, 0.0f, z) - parent.Position, -parentAngle);
RelativeX = inverse.x;
RelativeZ = inverse.z;
RelativeAngle = angle - parentAngle;
Bake(parent.BakedMatrix);
}
// Use this for initialization
void Start()
{
_rootPoint = new MotionPoint(0, 0, 0); //Can be anything
_animationList = new List <Animation>();
Util.FindChildrenWithAnimation(transform, _animationList);
_ikList = new List <IterativeIK>();
FindChildrenIKs(transform, _ikList);
_walkController = new WalkController(transform, null, _rootPoint, _minCyclesPerSecond, _maxCyclesPerSecond, _heightCorrection);
}
private void MotionItem_PointInserted(int index, MotionPoint point)
{
MotionPointView view = CreatePointViewFromData(point);
//Add to collection
pointViewList.Insert(index, view);
dataToViewDict.Add(point, view);
//Update UI
UpdateGraphLine();
EditorContext.MarkUnsaved();
}
//PointViews
private MotionPointView CreatePointViewFromData(MotionPoint motionPoint)
{
MotionPointView view = new MotionPointView(EditorContext, motionPoint);
PointCanvas.Children.Add(view);
//MainPoint
view.Data_MainPointChanged(motionPoint.MainPoint);
//SubPoints
for (int i = 0; i < motionPoint.SubPoints.Length; ++i)
{
view.Data_SubPointChanged(i, motionPoint.SubPoints[i]);
}
return(view);
}
private void MotionItem_PointRemoved(MotionPoint point)
{
MotionPointView view = dataToViewDict[point];
RemovePointView(view);
//Remove from collection
pointViewList.Remove(view);
dataToViewDict.Remove(point);
view.Dispose();
//Update UI
UpdateGraphLine();
EditorContext.MarkUnsaved();
}
//Points
private void CreatePointWithInterpolation(int index, Vector2 position)
{
//Collect prev, next points
MotionPointView prevPointView = pointViewList[index - 1];
MotionPointView nextPointView = pointViewList[index];
float prevNextDeltaX = nextPointView.Data.MainPoint.x - prevPointView.Data.MainPoint.x;
float prevDeltaX = position.x - prevPointView.Data.MainPoint.x;
float nextDeltaX = nextPointView.Data.MainPoint.x - position.x;
float weight = (position.x - prevPointView.Data.MainPoint.x) / prevNextDeltaX;
MotionPoint point = new MotionPoint();
//Calculate data
float mainPointX = position.x;
float subPoint0X = position.x - prevDeltaX * (1f - weight) * 0.5f;
float subPoint1X = position.x + nextDeltaX * weight * 0.5f;
Vector2 subPoint0 = new Vector2(subPoint0X, EditingMotionData.GetMotionValue(subPoint0X)) - point.MainPoint.ToVector2();
Vector2 subPoint1 = new Vector2(subPoint1X, EditingMotionData.GetMotionValue(subPoint1X)) - point.MainPoint.ToVector2();
Vector2 subPoint0Delta = subPoint0 - position;
Vector2 subPoint1Delta = subPoint1 - position;
//subPoint의 각도로부터 90도씩 꺾인 각도를 구한다
float subPoint0Angle = Mathf.Atan2(subPoint0Delta.y, subPoint0Delta.x) * Mathf.Rad2Deg + 90f;
float subPoint1Angle = Mathf.Atan2(subPoint1Delta.y, subPoint1Delta.x) * Mathf.Rad2Deg - 90f;
//그리고 둘이 섞었다가 다시 분리한다
float averAngle = (subPoint0Angle + subPoint1Angle) * 0.5f;
subPoint0Angle = (averAngle - 90f) * Mathf.Deg2Rad;
subPoint1Angle = (averAngle + 90f) * Mathf.Deg2Rad;
subPoint0 = new Vector2(Mathf.Cos(subPoint0Angle), Mathf.Sin(subPoint0Angle)) * subPoint0Delta.magnitude;
subPoint1 = new Vector2(Mathf.Cos(subPoint1Angle), Mathf.Sin(subPoint1Angle)) * subPoint1Delta.magnitude;
//Apply
point.SetSubPoint(0, subPoint0.ToPVector2());
point.SetSubPoint(1, subPoint1.ToPVector2());
point.SetMainPoint(new PVector2(mainPointX, EditingMotionData.GetMotionValue(mainPointX)));
prevPointView.Data.SetSubPoint(1, prevPointView.Data.SubPoints[1] * (prevDeltaX / prevNextDeltaX));
nextPointView.Data.SetSubPoint(0, nextPointView.Data.SubPoints[0] * (nextDeltaX / prevNextDeltaX));
EditingMotionData.InsertPoint(index, point);
}
//--------------------------------------------------
// Constructors
//--------------------------------------------------
/// <summary>
///
/// </summary>
/// <param name="bodyTransform"></param>
/// <param name="balanceTransform"></param>
/// <param name="rootPoint"></param>
/// <param name="peakHeight"></param>
/// <param name="minCyclesPerSecond"></param>
/// <param name="maxCyclesPerSecond"></param>
/// <param name="heightCorrection"></param>
public WalkController(Transform bodyTransform, Transform balanceTransform, MotionPoint rootPoint, float minCyclesPerSecond, float maxCyclesPerSecond, float heightCorrection)
{
_bodyTransform = bodyTransform;
_balanceTransform = balanceTransform;
_balancePosition = Vector3.zero;
_rootPoint = rootPoint;
_legList = new List <LegData>();
FindChildFeet(_bodyTransform, _legList);
_minCyclesPerSecond = minCyclesPerSecond;
_maxCyclesPerSecond = maxCyclesPerSecond;
_heightCorrection = heightCorrection;
float[] peakHeights = new float[_legList.Count];
for (int i = 0; i < _legList.Count; i++)
{
peakHeights[i] = _legList[i].region.PeakHeight;
}
_gaitGenerator = new GaitGenerator(_legList.Count, peakHeights);
QueueDefaultGait();
}
/// <summary>
///
/// </summary>
/// <param name="futureRoot"></param>
/// <param name="motions"></param>
/// <param name="pointSpeeds"></param>
/// <param name="minPathTime"></param>
/// <param name="maxPointSpeed"></param>
private void AllLegPathFactors(MotionPoint futureRoot, out Motion[] motions, out float[] pointSpeeds, out float minPathTime, out float maxPointSpeed)
{
motions = new Motion[_legList.Count];
pointSpeeds = new float[_legList.Count];
minPathTime = float.PositiveInfinity;
maxPointSpeed = 0.0f;
//Compute the individual foot point motions as a result of applying the previous motion to the root
for (int i = 0; i < _legList.Count; i++)
{
LegData leg = _legList[i];
//Compute the motion that would have resulted in the leg's point moving to the future point
MotionPoint futurePoint = leg.point.RelativeTo(futureRoot);
motions[i] = Motion.Between(leg.point, futurePoint);
minPathTime = Mathf.Min(minPathTime, leg.region.BoundedPathFactors(leg.point, motions[i], _gaitGenerator.MaxStanceRatio(i), out pointSpeeds[i], _bodyTransform));
maxPointSpeed = Mathf.Max(maxPointSpeed, pointSpeeds[i]);
}
}
public void RemovePoint(MotionPoint point)
{
pointList.Remove(point);
PointRemoved?.Invoke(point);
}
public void InsertPoint(int index, MotionPoint point)
{
pointList.Insert(index, point);
PointInserted?.Invoke(index, point);
}
public void AddPoint(MotionPoint point)
{
InsertPoint(pointList.Count, point);
}
/// <summary>
/// Check if two point in time can physically be a proper vector
/// </summary>
/// <param name="start"></param>
/// <param name="potencialEnd"></param>
/// <returns></returns>
public bool IsProperVector(MotionPoint start, MotionPoint potencialEnd)
{
return(AreNeighbors(start, potencialEnd) && potencialEnd.IsMovePhisicallyPosible(start, _motionConfiguration.MotionMinDiff));
}
/// <summary> /// Check if two points are neighbors /// </summary> /// <param name="p1"></param> /// <param name="p2"></param> /// <returns></returns> public bool AreNeighbors(MotionPoint p1, MotionPoint p2) => _rooms[p1.Uid].IsNeighbor(p2.Uid);
private bool AreNeighbors(MotionPoint p1, MotionPoint p2) => _rooms?[p1.Uid]?._neighbors?.Contains(p2.Uid) ?? false;
