static public float DotProduct(ViQuaternion a, ViQuaternion b)
{
return(a.x * b.x +
a.y * b.y +
a.z * b.z +
a.w * b.w);
}
public override bool Equals(object o)
{
if (o is ViQuaternion)
{
ViQuaternion q = (ViQuaternion)o;
return((this.x == q.x) && (this.y == q.y) && (this.z == q.z) && (this.w == q.w));
}
return(false);
}
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;
}
}
}
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);
}
static public ViQuaternion Slerp(ViQuaternion a, ViQuaternion b, float t)
{
float fScale0, fScale1;
double dCos = ViQuaternion.DotProduct(a, b);
if ((1.0 - Math.Abs(dCos)) > 1e-6f)
{
double dTemp = Math.Acos(Math.Abs(dCos));
double dSin = Math.Sin(dTemp);
fScale0 = (float)(Math.Sin((1.0 - t) * dTemp) / dSin);
fScale1 = (float)(Math.Sin(t * dTemp) / dSin);
}
else
{
fScale0 = 1.0f - t;
fScale1 = t;
}
if (dCos < 0.0)
{
fScale1 = -fScale1;
}
return((a * fScale0) + (b * fScale1));
}
static public ViQuaternion FromAxisAngle(float x, float y, float z, float fRadians)
{
return(ViQuaternion.FromAxisAngle(new ViVector3(x, y, z), fRadians));
}
