public override void _Update(float deltaTime, ViVector3 target)
{
ViVector3 diffDir = target - Translate;
diffDir.Normalize();
if (_direction == diffDir)
{
_velocity = _direction * _speed;
}
else
{
ViVector3 rotateAxis = ViVector3.Cross(_direction, diffDir);
rotateAxis.Normalize();
const float STABLE = 0.0001f;
// 计算公式与变量定义
// V 线速度
// W 角速度
// A 侧向加速度
// R 运动半径
// W = V/R;
// A = (V*V)/R = W*W*R = V*W;
float angleDiff = ViVector3.Angle(diffDir, _direction);
float destW = 4.0f * Math.Abs((angleDiff + STABLE) / (_duration + STABLE));
float destA = destW * Speed;
float destLateralAngle = (float)Math.Atan2(destA, _gravity);
//
_rollSpd = 3.0f * (destLateralAngle - _lateralAngle + STABLE) / (_duration + STABLE);
if (destLateralAngle > _lateralAngle)
{
_lateralAngle = ViMathDefine.MoveTowards(_lateralAngle, destLateralAngle, _rollSpd * deltaTime);
}
else
{
_lateralAngle = destLateralAngle;
}
float currentA = (float)Math.Tan(_lateralAngle) * _gravity;
float currentW = currentA / Speed;
float deltaAngle = currentW * deltaTime;
//
ViQuaternion rotateQuat = ViQuaternion.FromAxisAngle(rotateAxis, deltaAngle);
ViVector3 newDir = rotateQuat * _direction;
newDir.Normalize();
_velocity = (newDir + _direction) * _speed * 0.5f;
if (ViVector3.Dot(ViVector3.Cross(_direction, newDir), ViVector3.Cross(newDir, diffDir)) < 0.0f) // 插值抖动
{
_lateralSign = 0.0f;
_direction = diffDir;
}
else
{
_direction = newDir;
_lateralSign = (rotateAxis.z > 0.0f) ? 1.0f : -1.0f;
}
}
}
static public ViVector3 operator *(ViQuaternion a, ViVector3 pt)
{
//ViQuaternion quatResult = a * new ViQuaternion(pt.x, pt.y, pt.z, 0) * a.GetUnitInverse();
//return new ViVector3(quatResult.x, quatResult.y, quatResult.z);
ViVector3 uv, uuv;
ViVector3 qvec = new ViVector3(a.x, a.y, a.z);
uv = ViVector3.Cross(qvec, pt);
uuv = ViVector3.Cross(qvec, uv);
uv *= (2.0f * a.w);
uuv *= 2.0f;
return(pt + uv + uuv);
}
public static void Convert(ViVector3 diretion, float roll, out ViVector3 horizDir, out ViVector3 normal)
{
horizDir = diretion;
horizDir.z = 0.0f;
horizDir.Normalize();
diretion.Normalize();
ViVector3 rotateAxis = ViVector3.Cross(diretion, ViVector3.UNIT_Z);
rotateAxis.Normalize();
ViQuaternion verticalRotateQuat = ViQuaternion.FromAxisAngle(rotateAxis, ViMathDefine.PI_HALF);
normal = verticalRotateQuat * diretion;
ViQuaternion rollRotateQuat = ViQuaternion.FromAxisAngle(diretion, roll);
normal = rollRotateQuat * normal;
}
public bool Update(float deltaTime, ViVector3 newDir)
{
if (_curDiretion == newDir)
{
return(false);
}
float angleDiff = ViVector3.Angle(newDir, _curDiretion);
float rotAngle = _rotSpd * deltaTime;
if (angleDiff <= rotAngle)
{
_curDiretion = newDir;
}
else
{
ViVector3 rotateAxis = ViVector3.Cross(_curDiretion, newDir);
rotateAxis.Normalize();
ViQuaternion rotateQuat = ViQuaternion.FromAxisAngle(rotateAxis, rotAngle);
_curDiretion = rotateQuat * _curDiretion;
}
return(true);
}