public static SVector3 GetRotVectorFromOrigin(SVector3 origin, SQuaternion q)
{
SQuaternion temq = new SQuaternion(origin.x, origin.y, origin.z, (Fix64)0);
SQuaternion finalq = q * temq * q.conjugated;
return(new SVector3(finalq.x, finalq.y, finalq.z));
}
public static SQuaternion AxisAngle(SVector3 axis, Fix64 angle)
{
axis.Normalize();
Fix64 x = (Fix64)(axis.x * Fix64.Sin(angle / (Fix64)2));
Fix64 y = (Fix64)(axis.y * Fix64.Sin(angle / (Fix64)2));
Fix64 z = (Fix64)(axis.z * Fix64.Sin(angle / (Fix64)2));
Fix64 w = (Fix64)Fix64.Cos(angle / (Fix64)2);
return(new SQuaternion(x, y, z, w));
}
public void LookAt(SVector3 lookAtPoint)
{
SVector3 dir = lookAtPoint - position;
//SVector3 norm = SVector3.Cross(forward, dir);
if(dir.sqrtMagnitude < Fix64.One / (Fix64)10)
return;
SVector3 norm = SVector3.Cross(forward, dir);
if(norm.sqrtMagnitude < Fix64.One / (Fix64)100 && SVector3.Dot(dir.normalized,forward) > (Fix64)0)
{
return;
}
else if(norm.sqrtMagnitude < Fix64.One / (Fix64)100 && SVector3.Dot(dir.normalized,forward) < (Fix64)0)
{
RotateAroundAxis(up, (Fix64)Fix64.Pi / (Fix64)2);
}
else
{
RotateAroundAxis(norm.normalized, SVector3.AngleFromTo(forward, dir));
}
}
public STransform()
{
_localRotation = SQuaternion.AxisAngle(SVector3.up, (Fix64)0);
_localPosition = SVector3.zero;
}
public void RotateAroundAxis(SVector3 axis, Fix64 angle)
{
rotation = SQuaternion.AxisAngle(axis, angle) * rotation;
}
public static SQuaternion AxisAngleInDeg(SVector3 axis, Fix64 angle)
{
Fix64 temAngle = angle / (Fix64)180 * (Fix64)Fix64.Pi;
return(AxisAngle(axis, temAngle));
}
public static SVector3 ProjectOnPlane(SVector3 a, SVector3 normal)
{
return(a - Project(a, normal));
}
public static SVector3 Project(SVector3 a, SVector3 normal)
{
Fix64 pl = Dot(a, normal);
return(pl * normal.normalized);
}
public static Fix64 AngleFromTo(SVector3 a, SVector3 b)
{
return((Fix64)Fix64.Acos(Dot(a.normalized, b.normalized)));
}
public static SVector3 Cross(SVector3 a, SVector3 b)
{
return(a.Cross(b));
}
public SVector3 Cross(SVector3 b)
{
return(new SVector3(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x));
}
public static Fix64 Dot(SVector3 a, SVector3 b)
{
return(a.Dot(b));
}
public Fix64 Dot(SVector3 other)
{
return(other.x * x + other.y * y + other.z * z);
}
public void Add(SVector3 b)
{
this.x += b.x;
this.y += b.y;
this.z += b.z;
}
public static SVector3 GetRotVectorFromX(SVector3 origin, SQuaternion q)
{
Fix64 length = origin.magnitude;
return(GetRotVectorFromOrigin(SVector3.right, q) * length);
}