public float Angle(Vec4 other)
{
var m = MathF.Abs(this.Dot(other));
var d = this.Magnitude() * other.Magnitude();
return(MathF.Acos(m / d));
}
private void UpdateTransform()
{
// Get the local coordinate system
var rotation = Matrix.RotationQuaternion(Entity.Transform.Rotation);
// Enforce the global up-vector by adjusting the local x-axis
var right = Vector3.Cross(rotation.Forward, upVector);
var up = Vector3.Cross(right, rotation.Forward);
// Stabilize
right.Normalize();
up.Normalize();
// Adjust pitch. Prevent it from exceeding up and down facing. Stabilize edge cases.
var currentPitch = MathUtil.PiOverTwo - MathF.Acos(Vector3.Dot(rotation.Forward, upVector));
pitch = MathUtil.Clamp(currentPitch + pitch, -MaximumPitch, MaximumPitch) - currentPitch;
Vector3 finalTranslation = translation;
finalTranslation.Z = -finalTranslation.Z;
finalTranslation = Vector3.TransformCoordinate(finalTranslation, rotation);
// Move in local coordinates
Entity.Transform.Position += finalTranslation;
// Yaw around global up-vector, pitch and roll in local space
Entity.Transform.Rotation *= Quaternion.RotationAxis(right, pitch) * Quaternion.RotationAxis(upVector, yaw);
}
public static float Angle(Vector3 from, Vector3 to)
{
float mod = from.sqrMagnitude * to.sqrMagnitude;
float dot = Mathf.Clamp(Vector3.Dot(from, to) / Mathf.Sqrt(mod), -1.0f, 1.0f);
return(MathF.Acos(dot) * Mathf.Rad2Deg);
}
internal static float[] CalculateAngleMatches(int channels, Vector3 direction)
{
float[] angleMatches = new float[channels];
float dirMagnitudeRecip = 1f / (direction.Length() + .0001f);
for (int channel = 0; channel < channels; ++channel)
{
Channel currentChannel = Listener.Channels[channel];
if (!currentChannel.LFE)
{
float sample = MathF.PI - MathF.Acos(Vector3.Dot(direction, currentChannel.SphericalPos) * dirMagnitudeRecip);
sample *= sample;
sample *= sample;
angleMatches[channel] = sample * sample; // Angle match modifier function is x^8
}
}
if (Listener.EnvironmentType == Environments.Theatre)
{
for (int channel = 0; channel < channels; ++channel)
{
angleMatches[channel] *= angleMatches[channel]; // Theatre angle match modifier function is x^16
}
}
return(angleMatches);
}
public static Quaternion Slerp(Quaternion q1, Quaternion q2, float t)
{
float num2;
float num3;
Quaternion quaternion;
float num = t;
float num4 = (((q1.x * q2.x) + (q1.y * q2.y)) + (q1.z * q2.z)) + (q1.w * q2.w);
bool flag = false;
if (num4 < 0f)
{
flag = true;
num4 = -num4;
}
if (num4 > 0.999999f)
{
num3 = 1f - num;
num2 = flag ? -num : num;
}
else
{
float num5 = MathF.Acos(num4);
float num6 = (1.0f / MathF.Sin(num5));
num3 = (MathF.Sin(((1f - num) * num5))) * num6;
num2 = flag ? ((-MathF.Sin((num * num5))) * num6) : ((MathF.Sin((num * num5))) * num6);
}
quaternion.x = (num3 * q1.x) + (num2 * q2.x);
quaternion.y = (num3 * q1.y) + (num2 * q2.y);
quaternion.z = (num3 * q1.z) + (num2 * q2.z);
quaternion.w = (num3 * q1.w) + (num2 * q2.w);
return(quaternion);
}
public static float Angle(Vector2 a, Vector2 b)
{
float dot = Dot(a, b);
float l = a.Length * b.Length;
return(MathF.Acos(dot / l));
}
public void TickCollisions(TickCollisionsEvent _)
{
// TODO: Do this via collision detection instead.
// TODO: Clean this up.
// Keeps sword rested on the ground.
if (this.stateMachine_.IsOnGround)
{
var bladeFromGroundY = -(this.playerRigidbody_.CenterY -
this.playerRigidbody_.BottomY);
bladeFromGroundY -= MathF.Abs(
TrigMath.LenDegY(this.handDis_, this.handDeg_));
var minAngle = MathF.Acos(bladeFromGroundY / this.bladeLength_) /
MathF.PI *
180;
var diffToGround = TrigMath.DifferenceInDegrees(this.swordDeg_, 270);
if (FloatMath.Abs(diffToGround) <= minAngle)
{
this.swordDeg_ = 270 + FloatMath.Sign(diffToGround) * minAngle;
if (FloatMath.Abs(this.swordDevVel_) > 2)
{
this.swordDevVel_ *= -.5f;
}
else
{
this.swordDevVel_ = 0;
}
}
}
}
public static int SolveCubic(float a, float b, float c, out float r0, out float r1, out float r2)
{
float p = b - a * a / 3;
float p3 = p * p * p;
float q = a * (2 * a * a - 9 * b) / 27 + c;
float d = q * q + 4 * p3 / 27;
float s = a / -3;
if (d >= 0)
{
float z = MathF.Sqrt(d);
float u = (-q + z) / 2;
float v = (-q - z) / 2;
u = MathF.Cbrt(u);
v = MathF.Cbrt(v);
r0 = s + u + v;
Unsafe.SkipInit(out r1);
Unsafe.SkipInit(out r2);
return(1);
}
else
{
float u = MathF.Sqrt(p / -3);
float v = MathF.Acos(-MathF.Sqrt(-27 / p3) * q / 2) / 3;
float m = MathF.Cos(v);
float n = MathF.Cos(v - MathF.PI / 2) * 1.732050808f;
r0 = s + u * 2 * m;
r1 = s + u * (-m - n);
r2 = s + u * (-m + n);
return(3);
}
}
private static IEnumerable <Entity> LoadAtoms(string file, Dictionary <string, PhongMaterial> materials)
{
var mol = MoleculeFactory.Create(file);
foreach (var a in mol.Atoms)
{
Debug.Assert(a.CovalentRadius != null, "a.CovalentRadius != null");
yield return(new Sphere(Translation(a.Location) * Scale(a.CovalentRadius.Value / 150f))
{
Material = materials[a.Symbol]
});
}
foreach (var b in mol.Bonds)
{
var start = b.Atom1.Location;
var end = b.Atom2.Location;
var lineVec = end - start;
var axis = Vector3.Normalize(Vector3.Cross(Vector3.UnitY, lineVec));
var angle = MathF.Acos(Vector3.Dot(Vector3.UnitY, Vector3.Normalize(lineVec)));
var mat2 = Matrix4x4.Transpose(Matrix4x4.CreateFromAxisAngle(axis, angle));
var transform = Translation(start) * mat2 * Scale(.1f, lineVec.Length(), .1f);
var cy = new Cylinder(transform)
{
IsClosed = true, Maximum = 1, Minimum = 0, Material = new PhongMaterial(Util.FromHex("#cccccc"))
};
yield return(cy);
}
}
/// <summary>
/// Adds an arc segment at the corner defined by the last path point and two specified points.
/// </summary>
public static void ArcTo(this Nvg nvg, Vector2D <float> p1, Vector2D <float> p2, float radius)
{
Vector2D <float> p0 = nvg.instructionQueue.EndPosition;
if (nvg.instructionQueue.Count == 0)
{
return;
}
float distToll = nvg.pixelRatio.DistTol;
if (Maths.PtEquals(p0, p1, distToll) ||
Maths.PtEquals(p1, p2, distToll) ||
Maths.DistPtSeg(p1, p0, p2) < distToll ||
radius < distToll)
{
LineTo(nvg, p1);
return;
}
Vector2D <float> d0 = p0 - p1;
Vector2D <float> d1 = p2 - p1;
d0 = Vector2D.Normalize(d0);
d1 = Vector2D.Normalize(d1);
float a = MathF.Acos(d0.X * d1.X + d0.Y * d1.Y);
float d = radius / MathF.Tan(a / 2.0f);
if (d > 10000.0f)
{
LineTo(nvg, p1);
return;
}
Winding dir;
Vector2D <float> valuesC;
float a0, a1;
if (Maths.Cross(d0, d1) > 0.0f)
{
float cx = p1.X + d0.X * d + d0.Y * radius;
float cy = p1.Y + d0.Y * d + -d0.X * radius;
a0 = MathF.Atan2(d0.X, -d0.Y);
a1 = MathF.Atan2(-d1.X, d1.Y);
dir = Winding.Cw;
valuesC = new Vector2D <float>(cx, cy);
}
else
{
float cx = p1.X + d0.X * d + -d0.Y * radius;
float cy = p1.Y + d0.Y * d + d0.X * radius;
a0 = MathF.Atan2(-d0.X, d0.Y);
a1 = MathF.Atan2(d1.X, -d1.Y);
dir = Winding.Ccw;
valuesC = new Vector2D <float>(cx, cy);
}
Arc(nvg, valuesC, radius, a0, a1, dir);
}
public void RecalculateAngles()
{
var dir = new Vector4(0, 0, -1, 1);
var newdir = (dir * Node.GetTransform.GlobalTransform).Xyz - Node.GetTransform.GlobalTransform.ExtractTranslation();
phi = (int)(MathF.Atan2(newdir.Z, newdir.X) * 180 / MathF.PI);
theta = (int)(MathF.Acos(newdir.Y / newdir.Xzy.Length) * 180 / MathF.PI);
}
public static float AngleBetweenVectors(Vector2f a, Vector2f b)
{
// ^ ^
// angle = cos-1(a.b)
return(MathF.Acos(Dot(Normalize(a), Normalize(b)))); //radians
// angle = cos-1(a.b / ||A|| * ||B||)
// return MathF.Acos(Dot(a, b) / LengthSqrt(a) * LengthSqrt(b));
}
/// <summary>
/// Represent vector as direction in scperical angles
/// </summary>
/// <param name="vector"></param>
/// <returns>vector2 x - phi, y - theta in radians</returns>
public static Vector2 ConvertVector2SphereAngles(Vector3 vector)
{
var result = Vector2.Zero;
result.X = MathF.Atan2(vector.Z, vector.X);
result.Y = MathF.Acos(vector.Y / vector.Xzy.Length);
// result.Z = vector.LengthFast;
return(result);
}
public static void Acos()
{
Assert.Equal(0.0f, MathF.Acos(1.0f));
AssertEqual(MathF.PI / 2.0f, MathF.Acos(0.0f), CrossPlatformMachineEpsilon * 10);
AssertEqual(MathF.PI, MathF.Acos(-1.0f), CrossPlatformMachineEpsilon * 10);
Assert.Equal(float.NaN, MathF.Acos(float.NaN));
Assert.Equal(float.NaN, MathF.Acos(float.PositiveInfinity));
Assert.Equal(float.NaN, MathF.Acos(float.NegativeInfinity));
}
public void SetHeight(int height, int offset = 0)
{
var length = this.GetMaxLength();
var targetAngle = MathF.Acos((float)height / length);
var targetAngleDegrees = (int)(targetAngle * (180.0 / MathF.PI)) + 45;
this.servo.SetTargetAngle(targetAngleDegrees + offset);
}
private static void SetPlaneEntityRotation(int modelUpAxis, Vector3 upVector, Entity entity)
{
var axisModelUp = new Vector3 {
[modelUpAxis] = 1f
};
var axisRotationVector = Vector3.Cross(axisModelUp, upVector);
var axisRotationAngle = MathF.Acos(Vector3.Dot(axisModelUp, upVector));
entity.Transform.Rotation = Quaternion.RotationAxis(axisRotationVector, axisRotationAngle);
}
public override Color Eval(Normal normal, Vec inDir, Vec outDir, Vec2D uv)
{
float thetaIn = MathF.Acos(Utility.NormalizedDot(normal.ToVec(), inDir));
float thetaOut = MathF.Acos(Utility.NormalizedDot(normal.ToVec(), outDir));
if (MathF.Abs(thetaIn - thetaOut) < this.thresholdAngleRad)
return this.pigment.getColor(uv);
else
return new Color(0f, 0f, 0f);
}
// [Benchmark]
public float Acos()
{
float result = 0f;
foreach (float value in array)
{
result += MathF.Acos(value);
}
return(result);
}
/// <summary>
/// <para>Spherically interpolates between two vectors.</para>
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="t"></param>
public static Vector3 Slerp(Vector3 from, Vector3 to, float t)
{
float dot = Dot(from, to);
Math.Clamp(dot, -1.0f, 1.0f);
float theta = MathF.Acos(dot) * t;
Vector3 RelativeVec = to - from * dot;
RelativeVec.Normalize();
return((from * MathF.Cos(theta)) + (RelativeVec * MathF.Sin(theta)));
}
public static double SimpleAngleBetweenTwoVectors(Vector3 a, Vector3 b)
{
float c = MathF.Acos((float)Vector3.Dot(Vector3.Normalize(a), Vector3.Normalize(b)));
if (ConvertRadiansToDegrees(MathF.Atan2(b.Z - a.Z, b.X - a.X)) < 90)
{
c = -c;
}
return(c);
}
public static float[] AnalazeVector(Vector2f vec)
{
float[] angles = new float[4];
for (int i = 0; i < 4; ++i)
{
angles[i] = MathF.Acos((vec.X * SidesVectors[i].X) + (vec.Y * SidesVectors[i].Y));
}
return(angles);
}
//计算车到任务点的距离(这里只包括取货或者放货)
public float getShortLength(Car car, Task task, TaskType taskType)
{
float shortLength = 0.0f;
int startNum = car.car_EdgeData.vertexNum2;
int endNum = 0;
if (taskType == TaskType.Load)
{
endNum = task.loadGoods_EdgeData.vertexNum1;
}
else
{
endNum = task.unloadGoods_EdgeData.vertexNum1;
}
//使用djstra算法
shortLength += CarDispatch.Tools.Tool.Djstl(this, startNum, endNum);
//计算剩余的长度（包括小车到所在线段终点的距离+货物到达所在线段起点的距离）
float lineLength1, lineLength2;
lineLength1 = lineLength2 = 0.0f;
Position carPos = car.carPos; //车的位置
Position carVertexPos = this.vertexes[startNum].vertexPos; //车所在曲线的终点位置
//求lineLength1
if (car.car_EdgeData.lineType == LineType.Curve)
{
float distance = MathF.Sqrt(MathF.Pow(carPos.x - carVertexPos.x, 2)
+ MathF.Pow(carPos.y - carVertexPos.y, 2));
//注意ACos返回的是弧度值
float degree = MathF.Acos(distance * 0.5f / curveRadius) * 2; //两点在圆弧上的圆心角度
lineLength1 = degree * curveRadius; //弧长 = 半径 * 弧度
}
else
{
lineLength1 = MathF.Sqrt(MathF.Pow(carPos.x - carVertexPos.x, 2)
+ MathF.Pow(carPos.y - carVertexPos.y, 2));
}
Position taskPos = task.loadGoodsPos;
Position taskVertexPos = this.vertexes[endNum].vertexPos;//取货位置所在曲线起点的位置
//求lineLength2
if (task.loadGoods_EdgeData.lineType == LineType.Curve)
{
float distance = MathF.Sqrt(MathF.Pow(taskPos.x - taskVertexPos.x, 2)
+ MathF.Pow(taskPos.y - taskVertexPos.y, 2));
//
float degree = MathF.Acos(distance * 0.5f / curveRadius) * 2;
lineLength2 = degree * curveRadius;
}
//总长度=lineLength1 + 中间路径 + lineLength2;
shortLength += lineLength1 + lineLength2;
return(shortLength);
}
private Matrix4 getLightViewMatrix(Camera camera, Light light)
{
var ld = light.Direction.Normalized();
var pitch = MathF.Acos(ld.Xz.Length);
var yaw = MathF.Atan(ld.X / ld.Z);
yaw = ld.Z > 0 ? yaw - MathF.PI : yaw;
return(Matrix4.CreateTranslation(-camera.Look) *
Matrix4.CreateRotationY(-yaw) *
Matrix4.CreateRotationX(pitch));
}
Quaternion CalculateRotation(float r, int Iphi, int Itheta)
{
var look = cameraDir.Xyz;
var dir = CalcPos(r, Iphi, Itheta);
Vector3 axis = Vector3.Cross(look, dir);
return(Quaternion.FromAxisAngle(axis, MathF.Acos(Vector3.Dot(dir, look))));
//return Quaternion.FromAxisAngle(Vector3.UnitX, MathHelper.ConvertGrad2Radians(Itheta)) * Quaternion.FromAxisAngle(Vector3.UnitY, MathHelper.ConvertGrad2Radians(-Iphi));
//return Quaternion.FromEulerAngles(MathHelper.ConvertGrad2Radians(Itheta), MathHelper.ConvertGrad2Radians(-Iphi),0);
}
private static Vector3 randomSphere()
{
float r1 = (float)rnd.NextDouble();
float r2 = (float)rnd.NextDouble();
float lat = MathF.Acos(2 * r1 - 1) - MathF.PI / 2;
float lon = 2 * MathF.PI * r2;
return(new Vector3(
MathF.Cos(lat) * MathF.Cos(lon),
MathF.Cos(lat) * MathF.Sin(lon),
MathF.Sin(lat)
));
}
/// <summary>
/// Updates the simulated 3D audio settings with
/// an <see cref="AudioEmitter"/> and <see cref="AudioListener"/>.
/// </summary>
/// <exception cref="InvalidOperationException">
/// You must call <see cref="Apply3D(AudioListener, AudioEmitter)"/>
/// at least once before <see cref="Play()"/> to be able to call it in the future.
/// </exception>
/// <remarks>
/// Calling this method automatically converts the sound to monoaural and
/// sets the speaker mix for any sound played by this cue to a value calculated with
/// the listener's and emitter's positions.
/// Any stereo information in the sound will be discarded.
/// </remarks>
public void Apply3D(AudioListener listener, AudioEmitter emitter)
{
if (listener == null)
{
throw new ArgumentNullException(nameof(listener));
}
if (emitter == null)
{
throw new ArgumentNullException(nameof(emitter));
}
if (_played && !_applied3D)
{
throw new InvalidOperationException(
"You must call Apply3D() on a Cue before calling Play() " +
"to be able to call Apply3D() after calling Play().");
}
var direction = listener.Position - emitter.Position;
lock (_engine.UpdateLock)
{
// Set the distance for falloff.
var distance = direction.Length();
var i = FindVariable("Distance");
_variables[i].SetValue(distance);
// Calculate the orientation.
if (distance > 0f)
{
direction /= distance;
}
var right = Vector3.Cross(listener.Up, listener.Forward);
var slope = Vector3.Dot(direction, listener.Forward);
var angle = MathHelper.ToDegrees(MathF.Acos(slope));
var j = FindVariable("OrientationAngle");
_variables[j].SetValue(angle);
if (_currentSound != null)
{
_currentSound.SetCuePan(Vector3.Dot(direction, right));
}
// Calculate doppler effect.
var relativeVelocity = emitter.Velocity - listener.Velocity;
relativeVelocity *= emitter.DopplerScale;
}
_applied3D = true;
}
private void updateLightViewMatrix(Vector3 direction, Vector3 center)
{
direction.Normalize();
float pitch = (float)MathF.Acos(new Vector2(direction.X, direction.Z).Length);
float yaw = (float)MathHelper.RadiansToDegrees(((float)Math.Atan(direction.X / direction.Z)));
yaw = direction.Z > 0 ? yaw - 180 : yaw;
lightViewMatrix = Matrix4.Identity;
lightViewMatrix *= Matrix4.CreateRotationX(pitch);
lightViewMatrix *= Matrix4.CreateRotationY((float)-MathHelper.DegreesToRadians(yaw));
lightViewMatrix *= Matrix4.CreateTranslation(-center);
shadowBox.UpdateLightMatrix(lightViewMatrix);
}
public static Branch Log(Rotor r)
{
float cos_ang = _mm_store_ss(r.P1);
float ang = MathF.Acos(cos_ang);
float sin_ang = MathF.Sin(ang);
var p1 = _mm_mul_ps(r.P1, Detail.rcp_nr1(_mm_set1_ps(sin_ang)));
p1 = _mm_mul_ps(p1, _mm_set1_ps(ang));
p1 = Sse41.IsSupported
? _mm_blend_ps(p1, _mm_setzero_ps(), 1)
: _mm_and_ps(p1, _mm_castsi128_ps(_mm_set_epi32(-1, -1, -1, 0)));
return(new Branch(p1));
}
private Vector2 GetTextureCoords(Vector3 position)
{
var phi = MathF.Atan2(position.X, position.Z);
var theta = MathF.Acos(position.Y);
if (phi < 0)
{
phi += 2 * MathF.PI;
}
var u = phi * (1 / (2 * MathF.PI));
var v = 1 - theta * (1 / MathF.PI);
return(new Vector2(u, v));
}
private Vector2 GetTextureCoords(Vector3 normal)
{
var phi = MathF.Atan2(normal.X, normal.Z);
var theta = MathF.Acos(normal.Y);
if (phi < 0)
{
phi += 2 * MathF.PI;
}
var u = phi * (1 / (2 * MathF.PI));
var v = 1 - theta * (1 / MathF.PI);
return(new Vector2(u, v));
}
