public static Quaternion AngleAxisToQuaternion(float _angle, Vector3 _axis)
{
float radinH = UMath.AngleToRadin(_angle / 2);
float sinH = Mathf.Sin(radinH);
float cosH = Mathf.Cos(radinH);
return(new Quaternion(_axis.x * sinH, _axis.y * sinH, _axis.z * sinH, cosH));
}
public static Matrix3x3 AngleAxisToRotateMatrix(float _angle, Vector3 _axis)
{
float radin = UMath.AngleToRadin(_angle);
float s = Mathf.Sin(radin);
float c = Mathf.Cos(radin);
float t = 1 - c;
float x = _axis.x;
float y = _axis.y;
float z = _axis.z;
return(new Matrix3x3(t * x * x + c, t * x * y - s * z, t * x * z + s * y,
t * x * y + s * z, t * y * y + c, t * y * z - s * x,
t * x * z - s * y, t * y * z + s * x, t * z * z + c));
}
public static Quaternion EulerToQuaternion(float _angleX, float _angleY, float _angleZ) //Euler Axis XYZ
{
float radinHX = UMath.AngleToRadin(_angleX);
float radinHY = UMath.AngleToRadin(_angleY);
float radinHZ = UMath.AngleToRadin(_angleZ);
float sinHX = Mathf.Sin(radinHX); float cosHX = Mathf.Cos(radinHX);
float sinHY = Mathf.Sin(radinHY); float cosHY = Mathf.Cos(radinHY);
float sinHZ = Mathf.Sin(radinHZ); float cosHZ = Mathf.Cos(radinHZ);
float qX = cosHX * sinHY * sinHZ + sinHX * cosHY * cosHZ;
float qY = cosHX * sinHY * cosHZ + sinHX * cosHY * sinHZ;
float qZ = cosHX * cosHY * sinHZ - sinHX * sinHY * cosHZ;
float qW = cosHX * cosHY * cosHZ - sinHX * sinHY * sinHZ;
return(new Quaternion(qX, qY, qZ, qW));
}
static Vector2 RayConeCalculate(GCone _cone, GRay _ray)
{
Vector2 distances = Vector2.one * -1;
Vector3 offset = _ray.origin - _cone.origin;
float RDV = Vector3.Dot(_ray.direction, _cone.normal);
float ODN = Vector3.Dot(offset, _cone.normal);
float cosA = Mathf.Cos(UMath.AngleToRadin(_cone.angle));
float sqrCosA = cosA * cosA;
float a = RDV * RDV - sqrCosA;
float b = 2f * (RDV * ODN - Vector3.Dot(_ray.direction, offset) * sqrCosA);
float c = ODN * ODN - Vector3.Dot(offset, offset) * sqrCosA;
float determination = b * b - 4f * a * c;
if (determination < 0)
{
return(distances);
}
determination = Mathf.Sqrt(determination);
distances.x = (-b + determination) / (2f * a);
distances.y = (-b - determination) / (2f * a);
return(distances);
}
public float GetRadius(float _height) => _height *Mathf.Tan(UMath.AngleToRadin(angle));