void SmoothIn(TentacleEndeffector target)
{
// position
Vector3 src = Tentacle.Endeffector.transform.position;
Vector3 dst = target.position;
Vector3 root = Tentacle.Base.transform.position;
Vector3 dsrc = (src - root);
Vector3 ddst = (dst - root);
Vector3 dsrc0 = dsrc.ZeroNormalize();
Vector3 ddst0 = ddst.ZeroNormalize();
Vector3 axis = (src.x < dst.x ? 1.0f : -1.0f) * new Vector3(0, 0, -1);
Dbg3 = axis;
float radprocent = Mathf.Max(0.0f, Vector3.Dot(dsrc0, ddst0));
Quaternion q = Quaternion.AngleAxis(Time.deltaTime * 10.0f * (1.0f - radprocent), axis);
float rdst = Mathf.Max(0.6f * Tentacle.LengthMax, ddst.magnitude);
float rtarget = (1.0f - radprocent) * 0.8f * Tentacle.LengthMax + radprocent * rdst;
float rnow = dsrc.magnitude;
float r = 0.8f * rnow + 0.2f * rtarget;
Dbg1 = root + r * dsrc0;
Dbg2 = root + q * (r * dsrc0);
this.endeffector.position = root + r * (q * dsrc0);
// normal
float theta = (1.0f - Mathf.Min(Mathf.Max(0.0f, src.y / Tentacle.LengthMax), 1.0f));
Vector3 n0 = root - src;
if (n0.x < 0)
{
theta *= -1.0f;
}
Vector3 n = Quaternion.AngleAxis(theta * 90.0f, new Vector3(0, 0, 1)) * n0;
this.endeffector.normal = n.ZeroNormalize();
// float lerpfact = Time.deltaTime * LERP_RATE;
// this.endeffector.position = Vector3.Lerp(this.endeffector.position, target.position, lerpfact);
// Vector3 delta = this.endeffector.position - Tentacle.Base.transform.position;
// float m = delta.magnitude;
// if(m >= RANGE_MULT*Tentacle.LengthMax) {
// this.endeffector.position = Tentacle.Base.transform.position + RANGE_MULT*Tentacle.LengthMax/m*delta;
// }
// // normal
// this.endeffector.normal = Vector3.Lerp(Tentacle.EndeffectorNormal, target.normal, lerpfact).ZeroNormalize(); // FIXME
}
void ApplyPose(AI.TentacleEndeffector pose)
{
Endeffector.rigidbody.AddForce(CurrentForce * (pose.position - Endeffector.transform.position).ZeroNormalize());
Endeffector.transform.rotation = Quaternion.FromToRotation(new Vector3(0, 1, 0), -pose.normal);
}
void FixedUpdate()
{
lastPose = ai.Pose();
ApplyPose(lastPose);
}