/// <summary>
/// Create a hue filter matrix using the given angle in degrees.
/// </summary>
/// <param name="degrees">The angle of rotation in degrees.</param>
/// <returns>The <see cref="Matrix4x4"/></returns>
public static Matrix4x4 CreateHueFilter(float degrees)
{
// Wrap the angle round at 360.
degrees = degrees % 360;
// Make sure it's not negative.
while (degrees < 0)
{
degrees += 360;
}
float radian = MathFExtensions.DegreeToRadian(degrees);
float cosRadian = (float)MathF.Cos(radian);
float sinRadian = (float)MathF.Sin(radian);
// The matrix is set up to preserve the luminance of the image.
// See http://graficaobscura.com/matrix/index.html
// Number are taken from https://msdn.microsoft.com/en-us/library/jj192162(v=vs.85).aspx
return(new Matrix4x4
{
M11 = .213F + (cosRadian * .787F) - (sinRadian * .213F),
M12 = .213F - (cosRadian * .213F) - (sinRadian * 0.143F),
M13 = .213F - (cosRadian * .213F) - (sinRadian * .787F),
M21 = .715F - (cosRadian * .715F) - (sinRadian * .715F),
M22 = .715F + (cosRadian * .285F) + (sinRadian * 0.140F),
M23 = .715F - (cosRadian * .715F) + (sinRadian * .715F),
M31 = .072F - (cosRadian * .072F) + (sinRadian * .928F),
M32 = .072F - (cosRadian * .072F) - (sinRadian * 0.283F),
M33 = .072F + (cosRadian * .928F) + (sinRadian * .072F),
M44 = 1
});
}
/// <summary>
/// This will calculate the Mobius transform that represents an isometry in the given geometry.
/// The isometry will rotate CCW by angle A about the origin, then translate the origin to P (and -P to the origin).
/// </summary>
public void Isometry(Geometry g, double angle, Complex P)
{
// As Don notes in the hypebolic case:
// Any isometry of the Poincare disk can be expressed as a complex function of z of the form:
// (T*z + P)/(1 + conj(P)*T*z), where T and P are complex numbers, |P| < 1 and |T| = 1.
// This indicates a rotation by T around the origin followed by moving the origin to P (and -P to the origin).
//
// I figured out that the other cases can be handled with simple variations of the C coefficients.
Complex T = new Complex(Math.Cos(angle), Math.Sin(angle));
A = T;
B = P;
D = 1;
switch (g)
{
case Geometry.Spherical:
{
C = Complex.Conjugate(P) * T * -1;
break;
}
case Geometry.Euclidean:
{
C = 0;
break;
}
case Geometry.Hyperbolic:
{
C = Complex.Conjugate(P) * T;
break;
}
}
}
/// <summary>
/// From the virtual math museum
/// </summary>
public static Vector3D Dini2(Vector3D disk)
{
Vector3D uv = DiskToUpper(disk);
double u = Math.Log(uv.Y);
//double v = DonHatch.acosh( 1 + ( Math.Pow( uv.X, 2 ) + 0 ) / ( 2 * Math.Pow( uv.Y, 2 ) ) ) ;
//if( uv.X < 0 )
// v *= -1;
double v = uv.X;
if (u <= -4 || u > 4 ||
v < -6 * Math.PI || v > 6 * Math.PI)
{
return(Infinity.InfinityVector);
}
double psi = 0.5;
psi *= Math.PI;
double sinpsi = Math.Sin(psi);
double cospsi = Math.Cos(psi);
double g = (u - cospsi * v) / sinpsi;
double s = Math.Exp(g);
double r = (2 * sinpsi) / (s + 1 / s);
double t = r * (s - 1 / s) * 0.5;
return(new Vector3D(u - t, r * Math.Cos(v), r * Math.Sin(v)));
}
// NOTE: angle should be in radians.
public void RotateAboutAxis(Vector3D axis, double angle)
{
// normalize the axis
axis.Normalize();
double _x = axis.X;
double _y = axis.Y;
double _z = axis.Z;
// build the rotation matrix - I got this from http://www.makegames.com/3dRotation/
double c = Math.Cos(angle);
double s = -1 * Math.Sin(angle);
double t = 1 - c;
double[,] mRot = new double[, ]
{
{ 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 },
};
double x = this.X;
double y = this.Y;
double z = this.Z;
// do the multiplication
this = new Vector3D(
mRot[0, 0] * x + mRot[1, 0] * y + mRot[2, 0] * z,
mRot[0, 1] * x + mRot[1, 1] * y + mRot[2, 1] * z,
mRot[0, 2] * x + mRot[1, 2] * y + mRot[2, 2] * z);
}
/// <summary>
/// Get the length of the side of a triangle opposite alpha, given the three angles of the triangle.
/// NOTE: This does not work in Euclidean geometry!
/// </summary>
public static double GetTriangleSide(Geometry g, double alpha, double beta, double gamma)
{
switch (g)
{
case Geometry.Spherical:
{
// Spherical law of cosines
return(Math.Acos((Math.Cos(alpha) + Math.Cos(beta) * Math.Cos(gamma)) / (Math.Sin(beta) * Math.Sin(gamma))));
}
case Geometry.Euclidean:
{
// Not determined in this geometry.
Debug.Assert(false);
return(0.0);
}
case Geometry.Hyperbolic:
{
// Hyperbolic law of cosines
// http://en.wikipedia.org/wiki/Hyperbolic_law_of_cosines
return(DonHatch.acosh((Math.Cos(alpha) + Math.Cos(beta) * Math.Cos(gamma)) / (Math.Sin(beta) * Math.Sin(gamma))));
}
}
// Not determined in this geometry.
Debug.Assert(false);
return(0.0);
}
public static Polygon[] BuildDuoprism(int num)
{
double angleInc = 2 * Math.PI / num;
// Torus in two directions.
List <Polygon> polys = new List <Polygon>();
double angle1 = angleInc / 2;
for (int i = 0; i < num; i++)
{
double angle2 = angleInc / 2;
for (int j = 0; j < num; j++)
{
List <Vector3D> polyPoints = new List <Vector3D>();
polyPoints.Add(new Vector3D(
Math.Cos(angle2),
Math.Sin(angle2),
Math.Cos(angle1),
Math.Sin(angle1)));
polyPoints.Add(new Vector3D(
Math.Cos(angle2),
Math.Sin(angle2),
Math.Cos(angle1 + angleInc),
Math.Sin(angle1 + angleInc)));
polyPoints.Add(new Vector3D(
Math.Cos(angle2 + angleInc),
Math.Sin(angle2 + angleInc),
Math.Cos(angle1 + angleInc),
Math.Sin(angle1 + angleInc)));
polyPoints.Add(new Vector3D(
Math.Cos(angle2 + angleInc),
Math.Sin(angle2 + angleInc),
Math.Cos(angle1),
Math.Sin(angle1)));
Polygon poly = new Polygon();
poly.CreateEuclidean(polyPoints.ToArray());
polys.Add(poly);
angle2 += angleInc;
}
angle1 += angleInc;
}
// Nice starting orientation.
Matrix4D m1 = Matrix4D.MatrixToRotateinCoordinatePlane(Math.PI / 8, 0, 2);
Matrix4D m2 = Matrix4D.MatrixToRotateinCoordinatePlane(-Math.PI / 4, 1, 2);
foreach (Polygon poly in polys)
{
poly.Rotate(m1);
poly.Rotate(m2);
}
return(polys.ToArray());
}
public static Vec2 Rotate(this Vec2 v, float angle)
{
var x = v.X * (float)Math.Cos(angle) - v.Y * (float)Math.Sin(angle);
var y = v.X * (float)Math.Sin(angle) + v.Y * (float)Math.Cos(angle);
return(new Vec2(x, y));
}
public void Set(Table table, float fc, float q)
{
// Console.Log("\tfilter set fc", this.fc , "->", fc, "q", this.q, "->", q);
this.fc = fc;
this.q = q;
// https://github.com/FluidSynth/fluidsynth/blob/29c668683f43e3b8b4aab0b8aa73cb02aacd6fcb/src/rvoice/fluid_iir_filter.c#L382
// filter will defunct in 22050Hz sample rate
float maxFc = .45f * table.sampleRate;
if (fc > maxFc)
{
fc = maxFc;
}
else if (fc < 5)
{
fc = 5;
}
gain = 1.0 / Math.Sqrt(q);
// https://github.com/FluidSynth/fluidsynth/blob/29c668683f43e3b8b4aab0b8aa73cb02aacd6fcb/src/rvoice/fluid_iir_filter.c#L278
// previous simple bipolar lowpass is faulty when fc is large and should not be used:
// http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
double omega = Table.Pi2 * fc * table.sampleRateRecip;
double sin = Math.Sin(omega);
double cos = Math.Cos(omega);
double alpha = sin / (2f * q);
double a0Recip = 1f / (1 + alpha);
a1 = -2f * cos * a0Recip;
a2 = (1f - alpha) * a0Recip;
b1 = (1f - cos) * a0Recip * gain;
b2 = b1 * .5f;
}
public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)
{
// Roll first, about axis the object is facing, then
// pitch upward, then yaw to face into the new heading
float sr, cr, sp, cp, sy, cy;
float halfRoll = roll * 0.5f;
sr = (float)SM.Sin(halfRoll); cr = (float)SM.Cos(halfRoll);
float halfPitch = pitch * 0.5f;
sp = (float)SM.Sin(halfPitch); cp = (float)SM.Cos(halfPitch);
float halfYaw = yaw * 0.5f;
sy = (float)SM.Sin(halfYaw); cy = (float)SM.Cos(halfYaw);
Quaternion result;
result.X = cy * sp * cr + sy * cp * sr;
result.Y = sy * cp * cr - cy * sp * sr;
result.Z = cy * cp * sr - sy * sp * cr;
result.W = cy * cp * cr + sy * sp * sr;
return(result);
}
/// <summary>
/// Calculates distance/bearing between two geographic locations on Rhumb line (loxodrome)
/// </summary>
/// <param name="origin">origin location in geographic degrees</param>
/// <param name="destination">destination location in geographic degrees</param>
/// <param name="radius">radius of a geographic sphere, in kilometers</param>
/// <remarks>radius defaults to Earth's mean radius</remarks>
public static GeoRhumb GetRhumb(GeoPoint origin, GeoPoint destination, double radius = GeoGlobal.Earths.Radius)
{
origin = origin.ToRadians();
destination = destination.ToRadians();
var dLat = (destination.Latitude - origin.Latitude);
var dLon = (destination.Longitude - origin.Longitude);
var tDestination = Math.Tan(Math.PI / 4 + destination.Latitude / 2);
var tOrigin = Math.Tan(Math.PI / 4 + origin.Latitude / 2);
var dPhi = Math.Log(tDestination / tOrigin); // E-W line gives dPhi=0
var q = (IsFinite(dLat / dPhi)) ? dLat / dPhi : Math.Cos(origin.Latitude);
// if dLon over 180° take shorter Rhumb across anti-meridian:
if (Math.Abs(dLon) > Math.PI)
{
dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
}
var distance = Math.Sqrt(dLat * dLat + q * q * dLon * dLon) * radius;
var bearing = Math.Atan2(dLon, dPhi);
return(new GeoRhumb {
Distance = distance, Bearing = bearing.ToDegreesNormalized()
});
}
public void AddSphere(Vector3 position, float size, Color4 color)
{
// Compute our step around each circle
float step = GameUtils.TwoPi / 32;
// Create the loop on the XY plane first
for (float a = 0f; a < GameUtils.TwoPi; a += step)
{
AddLine(position + new Vector3((float)Math.Cos(a), (float)Math.Sin(a), 0f) * size,
position + new Vector3((float)Math.Cos(a + step), (float)Math.Sin(a + step), 0f) * size, color);
}
// Next on the XZ plane
for (float a = 0f; a < GameUtils.TwoPi; a += step)
{
AddLine(position + new Vector3((float)Math.Cos(a), 0f, (float)Math.Sin(a)) * size,
position + new Vector3((float)Math.Cos(a + step), 0f, (float)Math.Sin(a + step)) * size, color);
}
// Finally on the YZ plane
for (float a = 0f; a < GameUtils.TwoPi; a += step)
{
AddLine(position + new Vector3(0f, (float)Math.Cos(a), (float)Math.Sin(a)) * size,
position + new Vector3(0f, (float)Math.Cos(a + step), (float)Math.Sin(a + step)) * size, color);
}
}
/// <summary>
/// Calculates distance between two geographic locations on equirectangular map projection
/// <para>Using Pythagoras’ theorem </para>
/// </summary>
public static GeoDistance GetDistanceAppx(IGeoLocatable origin, IGeoLocatable destination, double radius = GeoGlobal.Earths.Radius)
{
origin = origin.ToRadians();
destination = destination.ToRadians();
var dLat = (destination.Latitude - origin.Latitude);
var dLon = (destination.Longitude - origin.Longitude);
var x = (dLon) * Math.Cos((origin.Latitude + destination.Latitude) / 2);
var y = (dLat);
var distance = Math.Sqrt(x * x + y * y) * radius;
return(new GeoDistance {
Kilometers = distance
});
//return distance;
//var yMin = Math.Min(origin.Latitude, destination.Latitude);
//var yMax = Math.Max(origin.Latitude, destination.Latitude);
//var xMin = Math.Min(origin.Longitude, destination.Longitude);
//var xMax = Math.Max(origin.Longitude, destination.Longitude);
//var yDelta = (yMax - yMin) * (yMax - yMin);
//var xDelta = (xMax - xMin) * (xMax - xMin);
//var distance = Math.Sqrt(xDelta + yDelta);
//return new GeoDistance { Degrees = distance };
}
public IPhysicsBody CreateCannonBall(int x, int y, double angle, double velocity, ICollider cannonBall)
{
var vx = Math.Cos((angle) * Math.PI / 180) * velocity;
var vy = Math.Sin((angle) * Math.PI / 180) * velocity;
var offvx = Math.Cos((angle) * Math.PI / 180) * 4d * 16d;
var offvy = Math.Sin((angle) * Math.PI / 180) * 3d * 16d;
var fixDef = new FixtureDef();
fixDef.density = 1;
fixDef.friction = 1;
fixDef.restitution = .6;
var bodyDef = new BodyDef();
bodyDef.type = BodyType.DYNAMIC;
bodyDef.position.x = this.PixelToMeter(x + offvx);
bodyDef.position.y = this.PixelToMeter(y + offvy);
var circleShape = new CircleShape();
circleShape.m_radius = 1.25d / 2d;
fixDef.shape = circleShape;
var body = this.World.createBody(bodyDef);
var fixture = body.createFixture(fixDef);
body.setUserData(cannonBall);
body.applyLinearImpulse(new Vec2(vx, vy), body.getWorldCenter());
return(new PhysicsBody(body));
}
public static Vector3D IntersectionPlaneLine(Vector3D planeNormal, Vector3D planePoint, Vector3D nl, Vector3D pl)
{
double signedDistance = DistancePointPlane(planeNormal, planePoint, pl);
planeNormal.Normalize();
Vector3D closest = pl - planeNormal * signedDistance;
Vector3D v1 = closest - pl;
Vector3D v2 = nl;
double angle = v1.AngleTo(v2);
nl.Normalize();
return(pl + nl * signedDistance / Math.Cos(angle));
// XXX - needs improvement.
/*
* Vector3D v1 = closest - pl;
* Vector3D v2 = nl;
* double angle = v1.AngleTo( v2 );
* Vector3D axis = v1.Cross( v2 );
* v1.RotateAboutAxis( axis, -angle );
* v1 /= Math.Cos( angle );
* return pl + v1;*/
}
/// <summary>
/// Rotate CCW in the XY plane by an angle in radians.
/// </summary>
public void RotateXY(double angle)
{
double component1 = X;
double component2 = Y;
X = Math.Cos(angle) * component1 - Math.Sin(angle) * component2;
Y = Math.Sin(angle) * component1 + Math.Cos(angle) * component2;
}
/// <summary>
/// Converts to Cartesian coordinates.
/// </summary>
/// <returns>CartesianCoordinate.</returns>
public static CartesianCoordinate3D ToCartesian(SphericalCoordinate coordinate)
{
double x = coordinate.Radius * NMath.Sin(coordinate.Inclination.Radians) * NMath.Cos(coordinate.Azimuth.Radians);
double y = coordinate.Radius * NMath.Sin(coordinate.Inclination.Radians) * NMath.Sin(coordinate.Azimuth.Radians);
double z = coordinate.Radius * NMath.Cos(coordinate.Inclination.Radians);
return(new CartesianCoordinate3D(x, y, z, coordinate.Tolerance));
}
/// <summary>
/// Calculates the maximum latitude of a great circle path from origin location in direction of bearing angle
/// <para>using Clairaut’s formula</para>
/// </summary>
/// <param name="origin">origin location in geographic degrees</param>
/// <param name="bearing">bearing from origin in geographic degrees</param>
public static double GetLatitudeMax(IGeoLocatable origin, double bearing)
{
origin = origin.ToRadians();
bearing = bearing.ToRadians();
var latMax = Math.Acos(Math.Abs(Math.Sin(bearing) * Math.Cos(origin.Latitude)));
return(latMax);
}
/// <summary>
/// Converts to cylindrical coordinates.
/// </summary>
/// <returns>CylindricalCoordinate.</returns>
public static CylindricalCoordinate ToCylindrical(SphericalCoordinate coordinate)
{
double radius = coordinate.Radius * NMath.Sin(coordinate.Inclination.Radians);
double height = coordinate.Radius * NMath.Cos(coordinate.Inclination.Radians);
double azimuth = coordinate.Azimuth.Radians;
return(new CylindricalCoordinate(radius, height, azimuth, coordinate.Tolerance));
}
private static void HopfFibration(Tiling tiling)
{
int segDivisions = 10;
int circleDivisions = 125;
Shapeways mesh = new Shapeways();
HashSet <Vector3D> done = new HashSet <Vector3D>();
foreach (Tile tile in tiling.Tiles)
{
foreach (Segment seg in tile.Boundary.Segments)
{
if (done.Contains(seg.Midpoint))
{
continue;
}
// Subdivide the segment, and project points to S2.
Vector3D[] points = seg.Subdivide(segDivisions).Select(v => Spherical2D.PlaneToSphere(v)).ToArray();
foreach (Vector3D point in points)
{
// Get the hopf circle and add to mesh.
// http://en.wikipedia.org/wiki/Hopf_fibration#Explicit_formulae
double a = point.X;
double b = point.Y;
double c = point.Z;
double factor = 1 / (Math.Sqrt(1 + c));
if (Tolerance.Equal(c, -1))
{
continue;
}
List <Vector3D> circlePoints = new List <Vector3D>();
double angleInc = 2 * Math.PI / circleDivisions;
double angle = 0;
for (int i = 0; i <= circleDivisions; i++)
{
double sinTheta = Math.Sin(angle);
double cosTheta = Math.Cos(angle);
circlePoints.Add(new Vector3D(
(1 + c) * cosTheta,
a * sinTheta - b * cosTheta,
a * cosTheta + b * sinTheta,
(1 + c) * sinTheta));
angle += angleInc;
}
bool shrink = false;
ProjectAndAddS3Points(mesh, circlePoints.ToArray(), shrink);
}
done.Add(seg.Midpoint);
}
}
STL.SaveMeshToSTL(mesh.Mesh, @"D:\p4\R3\sample\out1.stl");
}
public static Vector3D Dini(Vector3D uv, double a, double b)
{
uv = DiskToUpper(uv);
// Eq 1.86 on p36 of book Backlund and Darboux Transformations
double eta = Math.PI / 2 - Math.PI / 20;
//double eta = Math.PI / 2;
double p = 1; // curvature
double x = DonHatch.acosh(uv.Y); // Used info on mathworld for tractrix to figure this out.
//double x = DonHatch.acosh( Math.Exp( DonHatch.acosh( ( uv.Y * uv.Y + 1 ) / ( 2 * uv.Y ) ) ) );
//double x = Math.Log( uv.Y );
double y = uv.X;
double pSinEta = p * Math.Sin(eta);
double chi = (x - y * Math.Cos(eta)) / pSinEta;
if (x <= -4 || x > 4 ||
y < -3 * Math.PI || y > 3 * Math.PI)
{
return(Infinity.InfinityVector);
}
Vector3D result = new Vector3D(
pSinEta * Sech(chi) * Math.Cos(y / p),
pSinEta * Sech(chi) * Math.Sin(y / p),
x - pSinEta * Math.Tanh(chi));
return(result);
/*
* System.Func<double, Complex> tractrix = new System.Func<double, Complex>(
* ( t ) =>
* {
* //return new Complex( t - Math.Tanh( t ), 1.0 / Math.Cosh( t ) );
* return new Complex( - Math.Sqrt( 1 - 1 / (t*t) ) + DonHatch.acosh( t ), 1.0 / t );
* } );
*
* double logy = Math.Log( uv.Y );
* //Complex tract = tractrix( logy );
* Complex tract = tractrix( uv.Y );
* return new Vector3D(
* a * Math.Cos( uv.X ) * tract.Imaginary,
* a * Math.Sin( uv.X ) * tract.Imaginary,
* a * tract.Real + b * uv.X );
*/
/*
* return new Vector3D(
* a * Math.Cos( uv.X ) / Math.Cosh( uv.Y ),
* a * Math.Sin( uv.X ) / Math.Cosh( uv.Y ),
* a * (uv.Y - Math.Tanh( uv.Y )) + b * uv.X ); */
/*return new Vector3D(
* a * Math.Cos( uv.X ) * Math.Sin( uv.Y ),
* a * Math.Sin( uv.X ) * Math.Sin( uv.Y ),
* a * (Math.Cos( uv.Y ) + Math.Log( Math.Tan( 0.5 * uv.Y ) )) + b * uv.X );*/
}
public static Transform2 Rotate(double r)
{
double c = SysMath.Cos(r);
double s = SysMath.Sin(r);
return(new Transform2Matrix(
c, -s, 0,
s, c, 0));
}
public static Transform2 Rotate(double px, double py, double r)
{
double c = SysMath.Cos(r);
double s = SysMath.Sin(r);
return(new Transform2Matrix(
c, -s, -px * c + py * s + px,
s, c, -px * s - py * c + py));
}
public static Vec2 RotateGrad(this Vec2 v, float angle, float radius)
{
angle /= 180 * (float)Math.PI;
var x = v.X * (float)Math.Cos(angle) * radius - v.Y * (float)Math.Sin(angle) * radius;
var y = v.X * (float)Math.Sin(angle) * radius + v.Y * (float)Math.Cos(angle) * radius;
return(new Vec2(x, y));
}
private (double width, double height) GetBoundingBoxOfRotatedRectangle(double rotation, double width, double height)
{
if (rotation == 0)
{
return(width, height);
}
var rot = Astrometry.ToRadians(rotation);
return(Math.Abs(width * Math.Sin(rot) + height * Math.Cos(rot)), Math.Abs(height * Math.Sin(rot) + width * Math.Cos(rot)));
}
public static double EdgeLength(int p, int q, int r)
{
double pip = PiOverNSafe(p);
double pir = PiOverNSafe(r);
double pi_hqr = Pi_hpq(q, r);
double edgeLength = 2 * DonHatch.acosh(Math.Cos(pip) * Math.Sin(pir) / Math.Sin(pi_hqr));
return(edgeLength);
}
/// <summary>
/// Maps a vector in the unit square to a point on the Clifford Torus.
/// Does not currently check the input point.
/// </summary>
public static Vector3D MapToClifford(Vector3D v)
{
v *= 2 * Math.PI;
Vector3D result = new Vector3D(
Math.Cos(v.X),
Math.Sin(v.X),
Math.Cos(v.Y),
Math.Sin(v.Y));
return(Math.Sqrt(0.5) * result);
}
private static Mesh S3Helicoid(int numFibers, Matrix4D mat, bool reciprocal)
{
List <H3.Cell.Edge> fiberList = new List <H3.Cell.Edge>();
double numRotations = 3;
double rotationRate = 2 * Math.PI * numRotations;
double aOffset = 2 * Math.PI / numFibers;
for (int i = 0; i <= numFibers; i++)
{
double mag = 1 / Math.Sqrt(2);
double angleAlongCore = aOffset * i;
Vector3D corePoint, ray;
double rotAngle = (double)i / numFibers * rotationRate;
if (reciprocal)
{
Utils.Swap(ref angleAlongCore, ref rotAngle);
//rotAngle = -1 * rotAngle;
}
double x = mag * Math.Sin(angleAlongCore);
double y = mag * Math.Cos(angleAlongCore);
double z = mag * Math.Sin(rotAngle);
double w = mag * Math.Cos(rotAngle);
if (!reciprocal)
{
corePoint = new Vector3D(x, y);
ray = new Vector3D(0, 0, z, w);
}
else
{
corePoint = new Vector3D(0, 0, x, y);
ray = new Vector3D(z, w) * -1;
}
fiberList.Add(new H3.Cell.Edge(corePoint - ray, corePoint + ray, order: false));
}
System.Func <H3.Cell.Edge, Vector3D[]> divider = e =>
{
// Unequal subdivision on hypersphere, but shouldn't matter.
Segment seg = Segment.Line(e.Start, e.End);
return(seg.Subdivide(numFibers).Select(p => Transform2(p, mat)).ToArray());
};
Mesh thinMesh;
List <Vector3D[]> boundaryPoints;
ThinMesh(fiberList.ToArray(), divider, out thinMesh, out boundaryPoints);
return(thinMesh);
}
public static Point CreatePolarPoint(double phi, double theta, double rho)
{
phi = phi.ToRadians();
theta = theta.ToRadians();
var xE = rho * SysMath.Sin(phi) * SysMath.Cos(theta);
var yE = rho * SysMath.Sin(phi) * SysMath.Sin(theta);
var zE = rho * SysMath.Cos(phi);
return(new Point(xE, yE, zE));
}
public void Update(float frameTime, float heightDifference, MovementInput input)
{
_velocity.Update(frameTime, input);
_lookDirection.Update(frameTime, input);
if (input.Jump && heightDifference < 0.1f)
{
_verticalVelocity = 1.0f;
}
if (_verticalVelocity > -10.0f)
{
_verticalVelocity -= 0.05f;
}
Rotation.X += _lookDirection.Down;
Rotation.X = Rotation.X.Clamp(-90, 90);
Rotation.Y += _lookDirection.Right;
Rotation.Y = Rotation.Y.RotationLock();
var radians = Rotation.Y * Constants.RadiansPerDegree;
var sideways = (Rotation.Y + 90) * Constants.RadiansPerDegree;
var forwardX = (float)Math.Sin(radians) * _velocity.NetForward;
var forwardZ = (float)Math.Cos(radians) * _velocity.NetForward;
var sidewaysX = (float)Math.Sin(sideways) * _velocity.NetRightward;
var sidewaysZ = (float)Math.Cos(sideways) * _velocity.NetRightward;
var netMovement = new Vector2(forwardX + sidewaysX, forwardZ + sidewaysZ);
var maxVelocity = Constants.MaxVelocity;
if (input.Sprint)
{
maxVelocity *= 3;
}
if (netMovement.Length() > maxVelocity)
{
netMovement = Vector2.Normalize(netMovement);
netMovement = Vector2.Multiply(netMovement, maxVelocity);
}
Position.X += netMovement.X;
Position.Y += _verticalVelocity;
Position.Z += netMovement.Y;
//Position.X += (float)Math.Sin(radians) * _velocity.NetForward;
//Position.Y += _velocity.NetUpward;
//Position.Z += (float)Math.Cos(radians) * _velocity.NetForward;
}
public static Matrix3x2 CreateRotation(float radians, Vector2 centerPoint)
{
Matrix3x2 result;
radians = (float)SM.IEEERemainder(radians, SM.PI * 2);
float c, s;
const float epsilon = 0.001f * (float)SM.PI / 180f; // 0.1% of a degree
if (radians > -epsilon && radians < epsilon)
{
// Exact case for zero rotation.
c = 1;
s = 0;
}
else if (radians > SM.PI / 2 - epsilon && radians < SM.PI / 2 + epsilon)
{
// Exact case for 90 degree rotation.
c = 0;
s = 1;
}
else if (radians < -SM.PI + epsilon || radians > SM.PI - epsilon)
{
// Exact case for 180 degree rotation.
c = -1;
s = 0;
}
else if (radians > -SM.PI / 2 - epsilon && radians < -SM.PI / 2 + epsilon)
{
// Exact case for 270 degree rotation.
c = 0;
s = -1;
}
else
{
// Arbitrary rotation.
c = (float)SM.Cos(radians);
s = (float)SM.Sin(radians);
}
float x = centerPoint.X * (1 - c) + centerPoint.Y * s;
float y = centerPoint.Y * (1 - c) - centerPoint.X * s;
// [ c s ]
// [ -s c ]
// [ x y ]
result.M11 = c; result.M12 = s;
result.M21 = -s; result.M22 = c;
result.M31 = x; result.M32 = y;
return(result);
}