public Quaternion SetFromCross(Vector3f v1, Vector3f v2)
{
float Dot = MathUtils.Clamp(v1.Dot(v2), -1f, 1f);
float angle = MathUtils.Acos(Dot);
return(SetFromAxisRad(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x, angle));
}
public static Vec3 RotateTowards(Vec3 current, Vec3 target, float maxRadiansDelta, float maxMagnitudeDelta)
{
float len1 = current.Magnitude();
float len2 = target.Magnitude();
if (len1 > 1e-6 && len2 > 1e-6)
{
Vec3 from = current / len1;
Vec3 to = target / len2;
float cosom = Dot(from, to);
if (cosom > 1 - 1e-6)
{
return(MoveTowards(current, target, maxMagnitudeDelta));
}
if (cosom < -1 + 1e-6)
{
Quat q = Quat.AngleAxis(maxRadiansDelta * MathUtils.RAD_TO_DEG, OrthoNormalVector(from));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
else
{
float angle = MathUtils.Acos(cosom);
Quat q = Quat.AngleAxis(MathUtils.Min(maxRadiansDelta, angle) * MathUtils.RAD_TO_DEG,
Normalize(Cross(from, to)));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
}
return(MoveTowards(current, target, maxMagnitudeDelta));
}
public Quaternion ExpSelf(float alpha)
{
float norm = Len();
float normExp = MathUtils.Pow(norm, alpha);
float theta = MathUtils.Acos(w / norm);
float coeff;
if (MathUtils.Abs(theta) < 0.001)
{
coeff = normExp * alpha / norm;
}
else
{
coeff = (float)(normExp * MathUtils.Sin(alpha * theta) / (norm * MathUtils.Sin(theta)));
}
w = (float)(normExp * MathUtils.Cos(alpha * theta));
x *= coeff;
y *= coeff;
z *= coeff;
NorSelf();
return(this);
}
public static Quaternion CreateLookAtQuaternion(Vector3f eye,
Vector3f center, Vector3f up, Quaternion dest)
{
if (dest == null)
{
dest = new Quaternion();
}
Vector3f temp1 = Vector3f.TMP();
Vector3f temp2 = Vector3f.TMP();
Vector3f forward = temp1.Set(center).SubtractSelf(eye).NormalizeSelf();
Vector3f negativeZ = temp2.Set(Vector3f.AXIS_Z()).NegateSelf();
float dot = negativeZ.Dot(forward);
if (MathUtils.Abs(dot + 1) < 0.000001f)
{
return(dest.Set(up.x, up.y, up.z, MathUtils.PI));
}
if (MathUtils.Abs(dot - 1) < 0.000001f)
{
return(dest.Set());
}
float rotAngle = MathUtils.Acos(dot);
Vector3f rotAxis = negativeZ.CrossSelf(forward).NormalizeSelf();
dest.Set(rotAxis, rotAngle);
return(dest);
}
public Quaternion SlerpSelf(Quaternion end, float alpha)
{
float dot = Dot(end);
float absDot = dot < 0 ? -dot : dot;
float scale0 = 1 - alpha;
float scale1 = alpha;
if ((1 - absDot) > 0.1)
{
float angle = MathUtils.Acos(absDot);
float invSinTheta = 1f / MathUtils.Sin(angle);
scale0 = (MathUtils.Sin((1 - alpha) * angle) * invSinTheta);
scale1 = (MathUtils.Sin((alpha * angle)) * invSinTheta);
}
if (dot < 0)
{
scale1 = -scale1;
}
x = (scale0 * x) + (scale1 * end.x);
y = (scale0 * y) + (scale1 * end.y);
z = (scale0 * z) + (scale1 * end.z);
w = (scale0 * w) + (scale1 * end.w);
return(this);
}
public static Vec3 SlerpUnclamped(Vec3 from, Vec3 to, float t)
{
float scale0, scale1;
float len2 = to.Magnitude();
float len1 = from.Magnitude();
Vec3 v2 = to / len2;
Vec3 v1 = from / len1;
float len = (len2 - len1) * t + len1;
float cosom = Dot(v1, v2);
if (cosom > 1 - 1e-6)
{
scale0 = 1 - t;
scale1 = t;
}
else if (cosom < -1 + 1e-6)
{
Vec3 axis = OrthoNormalVector(from);
Quat q = Quat.AngleAxis(180.0f * t, axis);
Vec3 v = q * from * len;
return(v);
}
else
{
float omega = MathUtils.Acos(cosom);
float sinom = MathUtils.Sin(omega);
scale0 = MathUtils.Sin((1 - t) * omega) / sinom;
scale1 = MathUtils.Sin(t * omega) / sinom;
}
v2 = (v2 * scale1 + v1 * scale0) * len;
return(v2);
}
public float Angle90(Vec2 vector)
{
Vec2 vec = Normalize(this);
float val = MathUtils.Abs(vec.Dot(Normalize(vector)));
val = val > 1 ? 1 : val;
return(MathUtils.Acos(val));
}
public float GetAngleAroundRad(float axisX, float axisY, float axisZ)
{
float d = Vector3f.Dot(this.x, this.y, this.z, axisX, axisY, axisZ);
float l2 = Quaternion.Len2(axisX * d, axisY * d, axisZ * d, this.w);
return(MathUtils.IsZero(l2) ? 0f
: (float)(2.0 * MathUtils.Acos(MathUtils.Clamp((float)(this.w / MathUtils.Sqrt(l2)), -1f, 1f))));
}
public Quaternion SetFromCross(float x1, float y1, float z1, float x2, float y2,
float z2)
{
float Dot = MathUtils.Clamp(Vector3f.Dot(x1, y1, z1, x2, y2, z2), -1f, 1f);
float angle = MathUtils.Acos(Dot);
return(SetFromAxisRad(y1 * z2 - z1 * y2, z1 * x2 - x1 * z2, x1 * y2 - y1 * x2, angle));
}
public float Angle()
{
Vec2 vec = Normalize(this);
float val = vec.x;
val = val > 1 ? 1 : val;
val = val < -1 ? -1 : val;
return(MathUtils.Acos(val));
}
public static Texture2D CreateTexture(GraphicsDevice device, int width, float startAngle, float stopAngle)
{
if (ptextures.ContainsKey(width))
{
if (ptextures[width].ContainsKey(stopAngle))
{
return(ptextures[width][stopAngle]);
}
}
startAngle = MathHelper.ToRadians(startAngle);
float stopAngleRadians = MathHelper.ToRadians(stopAngle);
Texture2D texture = new Texture2D(device, width, width);
float halfPoint = width / 2f;
Vector2 centre = new Vector2(halfPoint, halfPoint);
Vector2 topVector = new Vector2(halfPoint, 0) - centre;
topVector.Normalize();
Color[] data = new Color[width * width];
for (int x = 0; x < width; x++)
{
for (int y = 0; y < width; y++)
{
Vector2 currentVector = new Vector2(x, y) - centre;
currentVector.Normalize();
float angle = MathUtils.Acos(Vector2.Dot(currentVector, topVector));
if (x < halfPoint)
{
angle = MathHelper.ToRadians(360f) - angle;
}
float lerp;
if (angle > startAngle && angle < stopAngleRadians)
{
lerp = 0;
}
else
{
lerp = MathHelper.Clamp(halfPoint - Vector2.Distance(centre, new Vector2(x, y)), 0, 1);
}
data[x + (y * width)] = Color.Lerp(TransparentWhite, Color.White, lerp);
}
}
texture.SetData(data);
if (!ptextures.ContainsKey(width))
{
ptextures.Add(width, new Dictionary <float, Texture2D>());
}
ptextures[width].Add(stopAngle, texture);
return(texture);
}
public void SetDimensions(Vector3 normal, float width, Vector3 start, Vector3 end)
{
Vector3 direction = Vector3.Normalize(end - start);
float angle = MathUtils.Acos(Vector3.Dot(Vector3.Backward, direction));
if (direction.X < 0)
{
angle = MathHelper.ToRadians(360) - angle;
}
float length = Vector3.Distance(start, end);
Vector3 centre = (start + end) / 2;
SetDimensions(normal, centre, Vector3.Backward, width, length);
Rotate(angle);
}
public static float AngleBetweenYZ(Vector3f v1, float y, float z)
{
float angle;
float Dot = v1.y * y + v1.z * z;
float len1 = MathUtils.Sqrt(v1.y * v1.y + v1.z * v1.z);
float Len2 = MathUtils.Sqrt(y * y + z * z);
if (len1 == 0 || Len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(Dot / (len1 * Len2));
return(angle);
}
public static float AngleBetweenXY(Vector3f v1, float x, float y)
{
float angle;
float Dot = v1.x * x + v1.y * y;
float len1 = MathUtils.Sqrt(v1.x * v1.x + v1.y * v1.y);
float Len2 = MathUtils.Sqrt(x * x + y * y);
if (len1 == 0 || Len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(Dot / (len1 * Len2));
return(angle);
}
public static float AngleBetween(Vector3f v1, float x, float y, float z)
{
float angle;
float dot = Dot(v1, x, y, z);
float len1 = Len(v1);
float Len2 = MathUtils.Sqrt(x * x + y * y + z * z);
if (len1 == 0 || Len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(dot / (len1 * Len2));
return(angle);
}
public static float AngleBetween(Vector3f vectorA, float x_0, float y_1, float z_2)
{
float angle;
float dot = Dot(vectorA, x_0, y_1, z_2);
float len1 = Len(vectorA);
float len2 = MathUtils.Sqrt(x_0 * x_0 + y_1 * y_1 + z_2 * z_2);
if (len1 == 0 || len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(dot / (len1 * len2));
return(angle);
}
public static float AngleBetween(Vector3f vectorA, Vector3f other)
{
float angle;
float dot = Dot(vectorA, other);
float len1 = Len(vectorA);
float len2 = Len(other);
if (len1 == 0 && len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(dot / (len1 * len2));
return(angle);
}
public static float AngleBetweenXY(Vector3f vectorA, float x_0, float y_1)
{
float angle;
float dot = vectorA.x * x_0 + vectorA.y * y_1;
float len1 = MathUtils.Sqrt(vectorA.x * vectorA.x + vectorA.y
* vectorA.y);
float len2 = MathUtils.Sqrt(x_0 * x_0 + y_1 * y_1);
if (len1 == 0 || len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(dot / (len1 * len2));
return(angle);
}
public static float AngleBetweenYZ(Vector3f vectorA, float y_0, float z_1)
{
float angle;
float dot = vectorA.y * y_0 + vectorA.z * z_1;
float len1 = MathUtils.Sqrt(vectorA.y * vectorA.y + vectorA.z
* vectorA.z);
float len2 = MathUtils.Sqrt(y_0 * y_0 + z_1 * z_1);
if (len1 == 0 || len2 == 0)
{
return(0);
}
angle = MathUtils.Acos(dot / (len1 * len2));
return(angle);
}
public Triangle(Vector a, Vector b, Vector c)
{
A = a;
B = b;
C = c;
Ab = new Edge(a, b);
Bc = new Edge(b, c);
Ac = new Edge(a, c);
alpha = MathUtils.Acos((MathUtils.Pow(Bc.Length, 2) - MathUtils.Pow(Ac.Length, 2) - MathUtils.Pow(Ab.Length, 2)) / (-2 * Ac.Length * Ab.Length)); // Winkelberechnung mit Kosinussatz
beta = MathUtils.Acos((MathUtils.Pow(Ac.Length, 2) - MathUtils.Pow(Bc.Length, 2) - MathUtils.Pow(Ab.Length, 2)) / (-2 * Bc.Length * Ab.Length));
gamma = MathUtils.Acos((MathUtils.Pow(Ab.Length, 2) - MathUtils.Pow(Bc.Length, 2) - MathUtils.Pow(Ac.Length, 2)) / (-2 * Bc.Length * Ac.Length));
float ccX = (A.X * MathUtils.Sin(2 * alpha) + B.X * MathUtils.Sin(2 * beta) + C.X * MathUtils.Sin(2 * gamma)) / (MathUtils.Sin(2 * alpha) + MathUtils.Sin(2 * beta) + MathUtils.Sin(2 * gamma));
float ccY = (A.Y * MathUtils.Sin(2 * alpha) + B.Y * MathUtils.Sin(2 * beta) + C.Y * MathUtils.Sin(2 * gamma)) / (MathUtils.Sin(2 * alpha) + MathUtils.Sin(2 * beta) + MathUtils.Sin(2 * gamma));
circumcenter = new Vector(ccX, ccY);
radius = MathUtils.Sqrt(MathUtils.Pow(A.X - circumcenter.X, 2) + MathUtils.Pow(A.Y - circumcenter.Y, 2));
}
public Vector3f SlerpSelf(Vector3f tarGet, float alpha)
{
float dot = Dot(tarGet);
if (dot > 0.9995 || dot < -0.9995)
{
return(LerpSelf(tarGet, alpha));
}
float theta0 = MathUtils.Acos(dot);
float theta = theta0 * alpha;
float st = MathUtils.Sin(theta);
float tx = tarGet.x - x * dot;
float ty = tarGet.y - y * dot;
float tz = tarGet.z - z * dot;
float l2 = tx * tx + ty * ty + tz * tz;
float dl = st * ((l2 < 0.0001f) ? 1f : 1f / MathUtils.Sqrt(l2));
return(ScaleSelf(MathUtils.Cos(theta)).AddSelf(tx * dl, ty * dl, tz * dl).NorSelf());
}
public static MeshData CreateGeosphere(float radius, SubdivisionCount numSubdivisions)
{
var tempMesh = new MeshData {
Vertices = IcosahedronVertices.Select(p => new Vertex {
Position = p
}).ToList(),
Indices = IcosahedronIndices
};
var mh = new Subdivider();
for (var i = 0; i < (int)numSubdivisions; i++)
{
mh.Subdivide4(tempMesh);
}
// Project vertices onto sphere and scale.
for (var i = 0; i < tempMesh.Vertices.Count; i++)
{
// Project onto unit sphere.
var n = Vector3.Normalize(tempMesh.Vertices[i].Position);
// Project onto sphere.
var p = radius * n;
// Derive texture coordinates from spherical coordinates.
var theta = MathF.AngleFromXY(tempMesh.Vertices[i].Position.X, tempMesh.Vertices[i].Position.Z);
var phi = MathUtils.Acos(tempMesh.Vertices[i].Position.Y / radius);
var texC = new Vector2(theta / (2 * MathF.PI), phi / MathF.PI);
// Partial derivative of P with respect to theta
var tangent = new Vector3(
-radius * MathUtils.Sin(phi) * MathUtils.Sin(theta),
0,
radius * MathUtils.Sin(phi) * MathUtils.Cos(theta));
tangent.Normalize();
tempMesh.Vertices[i] = new Vertex(p, n, tangent, texC);
}
return(tempMesh);
}
public void GetAngleAxis(out Vec3f axis, out float angle)
{
Quatf q = this;
if (MathUtils.Absolute(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * (float)MathUtils.Acos(q.w);
var den = (float)MathUtils.Sqrt(1.0f - (q.w * q.w));
if (den > 0.0001f)
{
axis = q.XYZ / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = Vec3f.Up;
}
}
public float GetAxisAngleRad(Vector3f axis)
{
if (this.w > 1)
{
this.NorSelf();
}
float angle = (float)(2.0 * MathUtils.Acos(this.w));
double s = MathUtils.Sqrt(1 - this.w * this.w);
if (s < MathUtils.FLOAT_ROUNDING_ERROR)
{
axis.x = this.x;
axis.y = this.y;
axis.z = this.z;
}
else
{
axis.x = (float)(this.x / s);
axis.y = (float)(this.y / s);
axis.z = (float)(this.z / s);
}
return(angle);
}
public float GetAngleRad()
{
return((float)(2.0 * MathUtils.Acos((this.w > 1) ? (this.w / Len()) : this.w)));
}
public static float GetAngleBetween(Vector3 first, Vector3 second)
{
first.Normalize();
second.Normalize();
return(MathUtils.Acos(Vector3.Dot(first, second)));
}
public static float Angle(Vec3 from, Vec3 to)
{
return(MathUtils.Acos(MathUtils.Clamp(Dot(Normalize(@from), Normalize(to)), -1, 1)) * MathUtils.RAD_TO_DEG);
}
