public SPCRJointDynamicsConstraint(ConstraintType Type, SPCRJointDynamicsPoint PointA, SPCRJointDynamicsPoint PointB)
{
_Type = Type;
_PointA = PointA;
_PointB = PointB;
UpdateLength();
}
void CreationConstraintBendingVertical(
SPCRJointDynamicsPoint Point,
ref List <SPCRJointDynamicsConstraint> ConstraintList)
{
if (Point.transform.childCount != 1)
{
return;
}
var childA = Point.transform.GetChild(0);
if (childA.childCount != 1)
{
return;
}
var childB = childA.transform.GetChild(0);
var childPointB = childB.GetComponent <SPCRJointDynamicsPoint>();
if (childPointB != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Bending_Vertical,
Point,
childPointB));
}
var childPointA = childA.GetComponent <SPCRJointDynamicsPoint>();
if (childPointA != null)
{
CreationConstraintBendingVertical(childPointA, ref ConstraintList);
}
}
void SubdivideVerticalChain(SPCRJointDynamicsController controller, int NumInsert)
{
var rnd = new System.Random();
var RootList = new List <SPCRJointDynamicsPoint>(controller._RootPointTbl);
var OriginalPoints = controller._SubDivOriginalPoints;
var InsertedPoints = controller._SubDivInsertedPoints;
var IsFirstSubdivide = (OriginalPoints.Count == 0);
foreach (var rootPoint in RootList)
{
if (IsFirstSubdivide)
{
OriginalPoints.Add(rootPoint);
}
var parentPoint = rootPoint;
while (parentPoint.transform.childCount > 0)
{
var parentTransform = parentPoint.transform;
var points = parentTransform.GetComponentsInChildren <SPCRJointDynamicsPoint>();
if (points.Length < 2)
{
break;
}
var childPoint = points[1];
if (parentPoint == childPoint)
{
Debug.LogWarning("Infinite Loop!:" + parentPoint.name);
break;
}
if (IsFirstSubdivide)
{
OriginalPoints.Add(childPoint);
}
var childTransform = childPoint.transform;
SPCRJointDynamicsPoint newPoint = null;
for (int i = 1; i <= NumInsert; i++)
{
float weight = i / (NumInsert + 1.0f);
newPoint = CreateInterpolatedPoint(parentPoint, childPoint, weight, "VSubdiv_" + rnd.Next());
InsertedPoints.Add(newPoint);
newPoint.transform.SetParent(parentTransform);
parentTransform = newPoint.transform;
}
childTransform.SetParent(newPoint.transform);
parentPoint = childPoint;
}
}
}
public static SPCRJointDynamicsConstraint Create(
ConstraintType Type, SPCRJointDynamicsPoint PointA, SPCRJointDynamicsPoint PointB)
{
var result = new SPCRJointDynamicsConstraint(Type, PointA, PointB);
result.UpdateLength();
return(result);
}
SPCRJointDynamicsPoint CreateInterpolatedPoint(SPCRJointDynamicsPoint point0, SPCRJointDynamicsPoint point1, float weight0, string newName = "SubDivPoint")
{
var Transform0 = point0.transform;
var Transform1 = point1.transform;
var pos = Vector3.Lerp(Transform0.position, Transform1.position, weight0);
var rot = Quaternion.Slerp(Transform0.rotation, Transform1.rotation, weight0);
var obj = new GameObject(newName);
var newPoint = obj.AddComponent <SPCRJointDynamicsPoint>();
var objTransform = obj.transform;
objTransform.position = pos;
objTransform.rotation = rot;
return(newPoint);
}
public SPCRJointDynamicsPoint GetChildJointDynamicsPoint(SPCRJointDynamicsPoint Parent)
{
if (Parent != null)
{
for (int i = 0; i < Parent.transform.childCount; ++i)
{
var child = Parent.transform.GetChild(i).GetComponent <SPCRJointDynamicsPoint>();
if (child != null)
{
return(child);
}
}
}
return(null);
}
void ComputePointParameter(SPCRJointDynamicsPoint Point, int Depth)
{
_MaxPointDepth = Mathf.Max(_MaxPointDepth, Depth);
Point._Depth = (float)Depth;
Point._IsFixed = Point._Depth == 0;
for (int i = 0; i < Point.transform.childCount; ++i)
{
var ChildPoint = Point.transform.GetChild(i).gameObject.GetComponent <SPCRJointDynamicsPoint>();
if (ChildPoint != null)
{
ComputePointParameter(ChildPoint, Depth + 1);
}
}
}
void CreationConstraintBendingHorizontal(
SPCRJointDynamicsPoint PointA,
SPCRJointDynamicsPoint PointB,
ref List <SPCRJointDynamicsConstraint> ConstraintList)
{
if ((PointA == null) || (PointB == null))
{
return;
}
if (PointA == PointB)
{
return;
}
var childPointA = GetChildJointDynamicsPoint(PointA);
var childPointB = GetChildJointDynamicsPoint(PointB);
if ((childPointA != null) && (childPointB != null))
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Bending_Horizontal,
childPointA,
childPointB));
CreationConstraintHorizontal(childPointA, childPointB, ref ConstraintList);
}
else if ((childPointA != null) && (childPointB == null))
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Bending_Horizontal,
childPointA,
PointB));
}
else if ((childPointA == null) && (childPointB != null))
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Bending_Horizontal,
PointA,
childPointB));
}
}
void CreateConstraintStructuralVertical(SPCRJointDynamicsPoint Point, ref List <SPCRJointDynamicsConstraint> ConstraintList)
{
for (int i = 0; i < Point.transform.childCount; ++i)
{
var child = Point.transform.GetChild(i);
var child_point = child.gameObject.GetComponent <SPCRJointDynamicsPoint>();
if (child_point != null)
{
Point._RefChildPoint = child_point;
var LocalPosition = Point.transform.InverseTransformPoint(Point._RefChildPoint.transform.position);
Point._BoneAxis = LocalPosition.normalized;
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Structural_Vertical,
Point,
child_point));
CreateConstraintStructuralVertical(child_point, ref ConstraintList);
}
}
}
void CreationConstraintShear(
SPCRJointDynamicsPoint PointA,
SPCRJointDynamicsPoint PointB,
ref List <SPCRJointDynamicsConstraint> ConstraintList)
{
if ((PointA == null) || (PointB == null))
{
return;
}
if (PointA == PointB)
{
return;
}
var childPointA = GetChildJointDynamicsPoint(PointA);
var childPointB = GetChildJointDynamicsPoint(PointB);
var childPointA2 = GetChildJointDynamicsPoint(childPointA);
var childPointB2 = GetChildJointDynamicsPoint(childPointB);
var childPointA3 = GetChildJointDynamicsPoint(childPointA2);
var childPointB3 = GetChildJointDynamicsPoint(childPointB2);
if (childPointA != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Shear,
childPointA,
PointB));
}
else if (childPointA2 != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Shear,
childPointA2,
PointB));
}
else if (childPointA3 != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Shear,
childPointA3,
PointB));
}
if (childPointB != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Shear,
PointA,
childPointB));
}
else if (childPointB2 != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Shear,
PointA,
childPointB2));
}
else if (childPointB3 != null)
{
ConstraintList.Add(new SPCRJointDynamicsConstraint(
ConstraintType.Shear,
PointA,
childPointB3));
}
CreationConstraintShear(childPointA, childPointB, ref ConstraintList);
}
public SPCRJointDynamicsConstraint(ConstraintType Type, SPCRJointDynamicsPoint PointA, SPCRJointDynamicsPoint PointB) => (_Type, _PointA, _PointB) = (Type, PointA, PointB);
void SubdivideHorizontalChain(SPCRJointDynamicsController controller, int NumInsert)
{
var rnd = new System.Random();
var RootList = new List <SPCRJointDynamicsPoint>(controller._RootPointTbl);
var OriginalPoints = controller._SubDivOriginalPoints;
var InsertedPoints = controller._SubDivInsertedPoints;
var IsFirstSubdivide = (OriginalPoints.Count == 0);
int Count = RootList.Count;
int Start = controller._IsLoopRootPoints ? Count : (Count - 1);
for (int iroot = Start; iroot > 0; iroot--)
{
var root0 = RootList[iroot % Count];
var root1 = RootList[iroot - 1];
for (int iin = 1; iin <= NumInsert; iin++)
{
var point0 = root0;
var point1 = root1;
var parentTransform = root0.transform.parent;
float weight = iin / (NumInsert + 1.0f);
SPCRJointDynamicsPoint newRoot = null;
while (point0 != null && point1 != null)
{
if (IsFirstSubdivide && iin == 1)
{
if (!controller._IsLoopRootPoints && iroot == Start)
{
OriginalPoints.Add(point0);
}
OriginalPoints.Add(point1);
}
var newPoint = CreateInterpolatedPoint(point0, point1, weight, "HSubdiv_" + rnd.Next());
InsertedPoints.Add(newPoint);
var newTransform = newPoint.transform;
newTransform.SetParent(parentTransform);
parentTransform = newTransform;
SPCRJointDynamicsPoint[] points;
points = point0.transform.GetComponentsInChildren <SPCRJointDynamicsPoint>();
point0 = (points.Length > 1) ? points[1] : null;
points = point1.transform.GetComponentsInChildren <SPCRJointDynamicsPoint>();
point1 = (points.Length > 1) ? points[1] : null;
if (newRoot == null)
{
newRoot = newPoint;
RootList.Insert(iroot, newRoot);
}
}
}
}
controller._RootPointTbl = RootList.ToArray();
}
