void AppendBone(Transform bone, BoneParticle parendBone, Vector3 extraEndBoneOffset = default(Vector3))
{
BoneParticle particle = new BoneParticle();
if (bone != null)
{
particle.position = particle.prevPosition = bone.position;
}
else
{
//虚结点,应该不会出现虚结点没有父节点的情况……
particle.position = particle.prevPosition = parendBone.position + extraEndBoneOffset;
}
if (parendBone != null)
{
}
bones.Add(particle);
if (bone != null)
{
int childCount = bone.childCount;
if (childCount > 0)
{
for (int i = 0; i < childCount; ++i)
{
if (!InExcludtion(bone.GetChild(i)))
{
AppendBone(bone.GetChild(i), particle);
}
}
}
else
{
//虚结点
switch (extraEndPointType)
{
case ExtraEndParticle.Length:
AppendBone(null, particle, Vector3.Normalize(particle.position - parendBone.position) * extraEndPointDist);
break;
case ExtraEndParticle.Offset:
AppendBone(null, particle, extraEndPointOffset);
break;
default:
//do nothing no extra end bone appended;
break;
}
}
}
}
private void CrateChildBoneParticle(BoneParticle root)
{
int childCount = root.BoneTransform.childCount;
if (childCount > 0)
{
Transform child = root.BoneTransform.GetChild(0);
BoneParticle newParticle = new BoneParticle(child.gameObject, false, _boneList.Count, root);
_boneList.Add(newParticle);
CrateChildBoneParticle(newParticle);
}
}
void Constraint()
{
for (int iter = 0; iter < 10; ++iter)
{
// for (int i = 0; i < bones.Count; ++i)
// {
// //pos restraints
// }
for (int i = 1; i < bones.Count; ++i)
{
BoneParticle parentBone = bones [i - 1];
BoneParticle bone = bones [i];
Vector3 originOffset = bone.position - parentBone.position;
float originLength = Mathf.Sqrt(Vector3.Dot(originOffset, originOffset));
//需要保持原形状
//Stiffness_Bend,弯曲刚性
//弯曲刚性应该不影响长度，只影响方向，正常的算法是需要acos的，不过这里用两步来趋近
Matrix4x4 parentLocalToWorld = parentBone.transform.localToWorldMatrix;
parentLocalToWorld.SetColumn(3, parentBone.position); //parent的位置可能是有变化的，修正坐标转换矩阵
// Vector3 restPos = parentLocalToWorld.MultiplyPoint3x4 (bone.boneInitOffset);
Vector3 restPos = parentLocalToWorld.MultiplyPoint3x4(bone.initLocalPosition);
Vector3 bendStiffnessVector = restPos - bone.position;
bone.position += bendStiffnessVector * bone.stiffnessBend;
//此时两个particle之间的距离已经发生了变化，在计算线性刚性之前要修正一下,这个修正只影响子节点就可以
Vector3 bended = bone.position - parentBone.position;
float bendedLength = Mathf.Sqrt(Vector3.Dot(bended, bended));
bone.position += bended * (originLength - bendedLength) / bendedLength;
//Stiffness_Strecth,线性刚性
//维持长度，将骨骼长度修复回原长度
Vector3 delta = bone.position - parentBone.position;
float deltaLength = Mathf.Sqrt(Vector3.Dot(delta, delta));
float diff = (deltaLength - bone.boneInitLength) / (deltaLength * (parentBone.InvMass + bone.InvMass));
diff *= bone.stiffnessStretch;
parentBone.position += diff * delta * parentBone.InvMass;
bone.position -= diff * delta * bone.InvMass;
// bone.boneCurLength = Vector3.Distance (bone.position, parentBone.position);
// float diff = (Vector3.Dot (delta, delta) - bone.boneInitDelta.magnitude * bone.boneInitDelta.magnitude) / (Vector3.Dot (delta, delta) - bone.boneInitDelta.magnitude * bone.boneInitDelta.magnitude);
// diff = diff / (parentBone.invMass + bone.invMass);
// parentBone.position += diff * delta * parentBone.invMass;
// bone.position -= diff * delta * bone.invMass;
}
}
}
private void SetBoneToDefaultPos()
{
for (int i = 0; i < _boneList.Count; i++)
{
BoneParticle bp = _boneList[i];
if (bp.IsRoot)
{
bp.TargetPos = bp.BoneTransform.position;
continue;
}
bp.BoneTransform.localRotation = bp.DefaultRotate;
bp.BoneTransform.localPosition = bp.DefaultLocalPos;
bp.TargetPos = bp.BoneTransform.position;
}
}
public BoneParticle(GameObject obj, bool isRoot, int boneIdx = 0, BoneParticle parentBone = null)
{
GO = obj;
IsRoot = isRoot;
BoneTransform = obj.transform;
CurrPos = TargetPos = BoneTransform.position;
DefaultLocalPos = BoneTransform.localPosition;
BoneIdx = boneIdx;
ParentBone = parentBone;
if (parentBone != null)
{
DefaultDisToParent = Vector3.Distance(BoneTransform.position, parentBone.BoneTransform.position);
}
DefaultRotate = BoneTransform.localRotation;
}
private void LateUpdate()
{
SetBoneToDefaultPos();
for (int i = 0; i < _boneList.Count; i++)
{
BoneParticle bp = _boneList[i];
if (bp.IsRoot)
{
bp.CurrPos = bp.BoneTransform.position;
continue;
}
Vector3 X = bp.BoneTransform.right;
Vector3 forceDir = bp.TargetPos - bp.CurrPos;
float distanceToOPos = forceDir.magnitude;
forceDir = forceDir.normalized;
float sprint = 50.0f;
float forceStr = sprint / bp.BoneIdx * distanceToOPos;
float mass = 3.0f;
Vector3 parentToThisDir = (bp.CurrPos - bp.ParentBone.CurrPos).normalized;
//parentToThisDir = (bp.CurrPos - bp.ParentBone.CurrPos).normalized;
//bp.CurrPos = bp.ParentBone.CurrPos + parentToThisDir * bp.DefaultDisToParent;
forceDir = Vector3.Cross(parentToThisDir, forceDir).normalized;
forceDir = Vector3.Cross(forceDir, parentToThisDir).normalized;
Vector3 accelerate = (forceStr / mass) * forceDir;
bp.Velocity += accelerate * Time.deltaTime;
bp.Velocity = Vector3.Dot(bp.Velocity, forceDir) * 0.99f * forceDir;
Vector3 dynamicPos = bp.CurrPos + bp.Velocity * Time.deltaTime;
parentToThisDir = (dynamicPos - bp.ParentBone.CurrPos).normalized;
bp.CurrPos = bp.ParentBone.CurrPos + parentToThisDir * bp.DefaultDisToParent;
bp.BoneTransform.position = bp.CurrPos; //bp.CurrPos;
//bp.BoneTransform.LookAt(bp.ParentBone.CurrPos, _rootBone.BoneTransform.forward);
Vector3 lookDir = (bp.CurrPos - bp.ParentBone.CurrPos).normalized;
//lookDir = Vector3.Cross(bp.BoneTransform.right, lookDir);
bp.BoneTransform.rotation = QuaternionLookRotation(lookDir, X);
}
}
void Verlet(float deltaTime)
{
BoneParticle particle = null;
Vector3 posCache;
//TODO 第一个节点需要特殊处理
for (int i = 0; i < bones.Count; ++i)
{
particle = bones [i];
if (particle.kinematic)
{
particle.prevPosition = particle.position;
particle.position = particle.transform.position;
}
else
{
posCache = particle.position;
particle.position += (particle.position - particle.prevPosition) * (1 - particle.damp) + particle.force * deltaTime * deltaTime / particle.Mass;
particle.prevPosition = posCache;
}
}
}
void AppendBone(Transform bone, BoneParticle parentBone, Vector3 extraEndBoneOffset = default(Vector3))
{
BoneParticle particle = new BoneParticle();
if (bone != null)
{
particle.position = particle.prevPosition = bone.position;
particle.transform = bone;
particle.initLocalPosition = bone.localPosition;
}
else
{
//虚结点,应该不会出现虚结点没有父节点的情况……
particle.position = particle.prevPosition = parentBone.position + extraEndBoneOffset;
particle.initLocalPosition = parentBone.transform.InverseTransformPoint(particle.position);
}
if (parentBone != null)
{
particle.boneInitLength = Vector3.Distance(particle.position, parentBone.position);
particle.Mass = 1; //HACK 先将质量都设成1
}
else
{
//第一个骨骼这个节点是
particle.kinematic = true;
}
particle.damp = damp;
particle.stiffnessBend = stiffnessBend;
particle.stiffnessStretch = stiffnessStretch;
bones.Add(particle);
if (bone != null)
{
int childCount = bone.childCount;
if (childCount > 0)
{
for (int i = 0; i < childCount; ++i)
{
if (!InExcludtion(bone.GetChild(i)))
{
AppendBone(bone.GetChild(i), particle);
}
}
}
else
{
//虚结点
switch (extraEndPointType)
{
case ExtraEndParticle.Length:
AppendBone(null, particle, Vector3.Normalize(particle.position - parentBone.position) * extraEndPointDist);
break;
case ExtraEndParticle.Offset:
AppendBone(null, particle, extraEndPointOffset);
break;
default:
//do nothing no extra end bone appended;
break;
}
}
}
}
private void Awake()
{
_rootBone = new BoneParticle(this.gameObject, true, 0);
_boneList.Add(_rootBone);
CrateChildBoneParticle(_rootBone);
}