public LVector3 this[int index]
{
get
{
switch (index)
{
case 0: return(x);
case 1: return(y);
case 2: return(z);
default: throw new System.IndexOutOfRangeException("vector idx invalid" + index);
}
}
set
{
switch (index)
{
case 0: x = value; break;
case 1: y = value; break;
case 2: z = value; break;
default: throw new System.IndexOutOfRangeException("vector idx invalid" + index);
}
}
}
public LVector3 WorldToLocal(LVector3 vec)
{
var _x = Dot(x, vec);
var _y = Dot(y, vec);
var _z = Dot(z, vec);
return(new LVector3(_x, _y, _z));
}
public LMatrix33(LVector3 column0, LVector3 column1, LVector3 column2)
{
this.m00 = column0._x;
this.m01 = column1._x;
this.m02 = column2._x;
this.m10 = column0._y;
this.m11 = column1._y;
this.m12 = column2._y;
this.m20 = column0._z;
this.m21 = column1._z;
this.m22 = column2._z;
}
/// <summary>
/// 转换为角轴
/// </summary>
/// <param name="angle"></param>
/// <param name="axis"></param>
public void ToAngleAxis(out LFloat angle, out LVector3 axis)
{
angle = 2 * LMath.Acos(w);
if (angle == 0)
{
axis = LVector3.right;
return;
}
LFloat div = 1 / LMath.Sqrt(1 - w * w);
axis = new LVector3(x * div, y * div, z * div);
angle = angle * 180 / LMath.PI;
}
/// <summary>
/// 轴向旋转
/// </summary>
/// <param name="angle"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static LQuaternion AngleAxis(LFloat angle, LVector3 axis)
{
axis = axis.normalized;
angle = angle * LMath.Deg2Rad;
LQuaternion q = new LQuaternion();
LFloat halfAngle = angle * LFloat.half;
LFloat s = LMath.Sin(halfAngle);
q.w = LMath.Cos(halfAngle);
q.x = s * axis.x;
q.y = s * axis.y;
q.z = s * axis.z;
return(q);
}
private LVector3 MatrixToEuler(LMatrix33 m)
{
LVector3 v = new LVector3();
if (m[1, 2] < 1)
{
if (m[1, 2] > -1)
{
v.x = LMath.Asin(-m[1, 2]);
v.y = LMath.Atan2(m[0, 2], m[2, 2]);
v.z = LMath.Atan2(m[1, 0], m[1, 1]);
}
else
{
v.x = LMath.PI * LFloat.half;
v.y = LMath.Atan2(m[0, 1], m[0, 0]);
v.z = (LFloat)0;
}
}
else
{
v.x = -LMath.PI * LFloat.half;
v.y = LMath.Atan2(-m[0, 1], m[0, 0]);
v.z = (LFloat)0;
}
for (int i = 0; i < 3; i++)
{
if (v[i] < 0)
{
v[i] += LMath.PI2;
}
else if (v[i] > LMath.PI2)
{
v[i] -= LMath.PI2;
}
}
return(v);
}
/// <summary>
/// 两个向量之间的夹角
/// </summary>
/// <param name="viewVec"></param>
/// <param name="upVec"></param>
/// <returns></returns>
private static LMatrix33 LookRotationToMatrix(LVector3 viewVec, LVector3 upVec)
{
LVector3 z = viewVec;
LMatrix33 m = new LMatrix33();
LFloat mag = z.magnitude;
if (mag <= 0)
{
m = LMatrix33.identity;
}
z /= mag;
LVector3 x = Cross(upVec, z);
mag = x.magnitude;
if (mag <= 0)
{
m = LMatrix33.identity;
}
x /= mag;
LVector3 y = Cross(z, x);
m[0, 0] = x.x;
m[0, 1] = y.x;
m[0, 2] = z.x;
m[1, 0] = x.y;
m[1, 1] = y.y;
m[1, 2] = z.y;
m[2, 0] = x.z;
m[2, 1] = y.z;
m[2, 2] = z.z;
return(m);
}
/// <summary>
/// 注视旋转
/// </summary>
/// <param name="forward"></param>
/// <returns></returns>
public static LQuaternion LookRotation(LVector3 forward)
{
LVector3 up = LVector3.up;
return(LookRotation(forward, up));
}
public LAxis3D(LVector3 x, LVector3 y, LVector3 z)
{
this.x = x;
this.y = y;
this.z = z;
}
/// <summary>
/// 设置注视旋转
/// </summary>
/// <param name="view"></param>
public void SetLookRotation(LVector3 view)
{
this = LookRotation(view);
}
/// <summary>
/// 设置角度
/// </summary>
/// <param name="fromDirection"></param>
/// <param name="toDirection"></param>
public void SetFromToRotation(LVector3 fromDirection, LVector3 toDirection)
{
this = FromToRotation(fromDirection, toDirection);
}
public LVector3 LocalToWorld(LVector3 vec)
{
return(x * vec.x + y * vec.y + z * vec.z);
}
/// <summary>
/// 向量间的角度
/// </summary>
/// <param name="fromDirection"></param>
/// <param name="toDirection"></param>
/// <returns></returns>
public static LQuaternion FromToRotation(LVector3 fromDirection, LVector3 toDirection)
{
throw new IndexOutOfRangeException("Not Available!");
}
/// <summary>
/// 欧拉角转四元数
/// </summary>
/// <param name="euler"></param>
/// <returns></returns>
public static LQuaternion Euler(LVector3 euler)
{
return(Euler(euler.x, euler.y, euler.z));
}
public static LVector3Int ToLVector3Int(this LVector3 vec)
{
return new LVector3Int(vec.x.ToInt(), vec.y.ToInt(),vec.z.ToInt());
}
/// <summary>
/// <para>Sets a row of the matrix.</para>
/// </summary>
/// <param name="index"></param>
/// <param name="row"></param>
public void SetRow(int index, LVector3 row)
{
this[index, 0] = row.x;
this[index, 1] = row.y;
this[index, 2] = row.z;
}
/// <summary>
/// <para>Sets a column of the matrix.</para>
/// </summary>
/// <param name="index"></param>
/// <param name="column"></param>
public void SetColumn(int index, LVector3 column)
{
this[0, index] = column.x;
this[1, index] = column.y;
this[2, index] = column.z;
}
/// <summary>
/// 注视旋转
/// </summary>
/// <param name="forward"></param>
/// <param name="upwards"></param>
/// <returns></returns>
public static LQuaternion LookRotation(LVector3 forward, LVector3 upwards)
{
LMatrix33 m = LookRotationToMatrix(forward, upwards);
return(MatrixToQuaternion(m));
}
public LAxis2D(LVector3 x, LVector3 y)
{
this.x = x;
this.y = y;
}
