// Start is called before the first frame update
void Start()
{
DQuaternion.Test();
SetParameters(StartingInertia, StartingOmega, ApplyAdjustment);
}
public static double EnergyFromOrientation(DQuaternion o, DVector3 L, DVector3 I)
{
var ir = DQuaternion.Inverse(o);
DVector3 body_l = ir * L;
double e = body_l.x * body_l.x / I.x +
body_l.y * body_l.y / I.y +
body_l.z * body_l.z / I.z;
e *= .5f;
return(e);
}
void UpdateOrientation(targ_type dt)
{
DVector3 axis = Omega.normalized;
// Omega is in radians/sec, but AngleAxis need degrees.
targ_type w = (targ_type)(Omega.magnitude * (360 / (2 * Math.PI)));
// Note that in Unity, a positive angle of rotation is clockwise around the
// axis of rotation. This is unusual....
var inc = DQuaternion.AngleAxis(w * dt, axis);
PrevOrientation = Orientation;
Orientation = inc * Orientation;
}
void Update()
{
Quaternion q = new Quaternion();
q.SetFromToRotation(Vector3.up, DVector3.ToUnity(Body.Omega));
Omega.transform.localRotation = q;
Omega.SetLength((float)Body.Omega.magnitude / MasterScale);
AngularVelocityTrail.transform.localPosition = DVector3.ToUnity(Body.Omega / MasterScale);
InertiaEllipsoid.transform.rotation = DQuaternion.ToUnity(Body.Orientation);
AngularVelocityTrail.enabled = true;
}
public DQuaternion AdjustOrientation(DQuaternion initial)
{
DQuaternion cur = initial;
int niter = 0;
double ecur_i = EnergyFromOrientation(cur);
double del_i = Energy - ecur_i;
for (; ;)
{
double ecur = EnergyFromOrientation(cur);
double del = Energy - ecur;
if (Math.Abs(del) < Energy * .0001f)
{
break;
}
var next = Iterate(initial);
double ef = EnergyFromOrientation(next);
double delf = Energy - ef;
Debug.Assert(delf <= del);
cur = next;
niter++;
if (niter > 6)
{
Debug.Log("EL Sovler ran out of iterations.");
break;
}
}
if (niter > 0)
{
double ecur_f = EnergyFromOrientation(cur);
double del_f = Energy - ecur_f;
//Debug.Log(string.Format("ELSolver changed del from {0} to {1} out of {2} in {3} iter.",
// del_i, del_f, Energy, niter));
}
return(cur);
}
public void SetParameters(Vector3 inertia, Vector3 omega, bool apply_adjustment)
{
ConditionParameters(ref inertia, ref omega);
InitialOmega = DVector3.FromUnity(omega);
BodyOmega = InitialOmega; // Initial BTW is identity
bool b = ExtentsFromInertia(DVector3.FromUnity(inertia), out Extents);
if (b)
{
Debug.Assert((InertiaFromExtents(Extents) - DVector3.FromUnity(inertia)).magnitude < .001f);
}
else
{
Debug.Log(("Requested inertia values don't correspond to an ellipsoid object."));
}
ApplyAdjustment = apply_adjustment;
transform.localScale = DVector3.ToUnity(Extents);
Orientation = DQuaternion.identity;
PrevOrientation = DQuaternion.identity;
I = DVector3.FromUnity(inertia);
Debug.Log(string.Format("Inertia Values: ({0},{1},{2})", I.x, I.y, I.z));
// Initially the body and world coordinates match:
L.x = I.x * Omega.x;
L.y = I.y * Omega.y;
L.z = I.z * Omega.z;
Energy = ELSolver.EnergyFromOrientation(Orientation, L, I);
Debug.Assert(Energy == CurrentE());
ELSolver = new ELSolver(Energy, L, I);
UpdateCount = 0;
DumpParameters();
BodyParmsChanged?.Invoke();
}
private void Normalize()
{
// Renormalize the orientation, and make sure angular momentum
// and energy are being conserved.
Orientation.Normalize();
if (ApplyAdjustment)
{
Orientation = ELSolver.AdjustOrientation(Orientation);
var ir = DQuaternion.Inverse(Orientation);
var body_l = ir * L;
_BodyOmega.x = body_l.x / I.x;
_BodyOmega.y = body_l.y / I.y;
_BodyOmega.z = body_l.z / I.z;
}
}
void UpdateOmega(targ_type dt)
{
var body_omega = BodyOmega;
DVector3 body_omega_dt = new DVector3();
// Euler's equations for torque free motion.
// Even the the Unity coordinate system is left-handed, this still works.
// Uses explicit Euler integration...
body_omega_dt.x = (I.y - I.z) * body_omega.y * body_omega.z / I.x;
body_omega_dt.y = (I.z - I.x) * body_omega.z * body_omega.x / I.y;
body_omega_dt.z = (I.x - I.y) * body_omega.x * body_omega.y / I.z;
body_omega += body_omega_dt * dt;
BodyOmega = body_omega;
Debug.Assert(DVector3.Distance(body_omega, DQuaternion.Inverse(Orientation) * Omega) < 1.0e10);
}
// Perform a single iteration: Find the energy delta, measure the gradient WRT
// each perturbation. Find the one with the largest effect and apply a scaled adjustment
// to the orientation.
//
DQuaternion Iterate(DQuaternion cur_orientation)
{
double ecur = EnergyFromOrientation(cur_orientation);
double del = Energy - ecur;
double dm = 0;
double delbest = 0;
int ibest = 0;
for (int im = 0; im < Adjustments.Length; im++)
{
var o = Adjustments[im] * cur_orientation;
var ep = EnergyFromOrientation(o);
var cd = ep - ecur;
if (Math.Abs(cd) > dm)
{
ibest = im;
delbest = cd;
dm = Math.Abs(cd);
}
}
double factor = del / delbest;
if (factor < -4)
{
factor = -4; // There is no clamp for doubles.
}
else if (factor > 4)
{
factor = 4;
}
double angle = AdjustAngle * factor;
var adjust = DQuaternion.AngleAxis(angle, AdjustmentDirs[ibest]);
var next_orientation = adjust * cur_orientation;
return(next_orientation);
}
/// <summary>
///
/// </summary>
/// <param name="gameTime"></param>
void DrawInternal(GameTime gameTime)
{
var dev = Game.GraphicsDevice;
viewMatrix = DMatrix.LookAtRH(cameraPosition, DVector3.Zero, DVector3.UnitY);
DMatrix vvvM = viewMatrix;
viewMatrix.TranslationVector = DVector3.Zero;
var camPos = cameraPosition;
if (Game.InputDevice.IsKeyDown(Keys.LeftShift))
{
DVector3 cameraUp = cameraPosition / cameraPosition.Length();
DVector3 lookAtPoint = cameraUp * Config.earthRadius;
double length = Config.CameraDistance - Config.earthRadius;
var quat = DQuaternion.RotationAxis(DVector3.UnitY, FreeCamYaw) * DQuaternion.RotationAxis(DVector3.UnitX, FreeCamPitch);
var qRot = DMatrix.RotationQuaternion(quat);
var mat = DMatrix.Identity;
var xAxis = DVector3.TransformNormal(DVector3.UnitX, DMatrix.RotationAxis(DVector3.UnitY, Yaw));
xAxis.Normalize();
mat.Up = cameraUp;
mat.Right = xAxis;
mat.Forward = DVector3.Cross(xAxis, cameraUp);
mat.Forward.Normalize();
var matrix = qRot * mat;
var c = DVector3.Transform(new DVector3(0, 0, length), matrix);
var camPoint = new DVector3(c.X, c.Y, c.Z) + lookAtPoint;
camPos = camPoint;
viewMatrix = DMatrix.LookAtRH(camPoint, lookAtPoint, cameraUp);
vvvM = viewMatrix;
viewMatrix.TranslationVector = DVector3.Zero;
}
viewMatrixFloat = DMatrix.ToFloatMatrix(viewMatrix);
projMatrixFloat = DMatrix.ToFloatMatrix(projMatrix);
var viewDir = cameraPosition / cameraPosition.Length();
constBuffer.Data.ViewProj = viewMatrixFloat * projMatrixFloat;
constBuffer.Data.ViewPositionX = camPos.X;
constBuffer.Data.ViewPositionY = camPos.Y;
constBuffer.Data.ViewPositionZ = camPos.Z;
constBuffer.Data.Temp = new Vector4(0.0f, (float)Config.earthRadius, 0, 0);
constBuffer.Data.ViewDir = new Vector4((float)viewDir.X, (float)viewDir.Y, (float)viewDir.Z, 0);
constBuffer.UpdateCBuffer();
Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearClamp;
Game.GraphicsDevice.PixelShaderResources[1] = frame;
if (gridVertexBuffer != null)
{
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_POLY | GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_TEXTURED | (Config.ShowFrames ? GlobeFlags.SHOW_FRAMES : GlobeFlags.NONE),
Primitive.TriangleList,
BlendState.AlphaBlend,
RasterizerState.CullCW,
DepthStencilState.Readonly);
Game.GraphicsDevice.PixelShaderResources[0] = gridTex;
Game.GraphicsDevice.SetupVertexInput(gridVertexBuffer, gridIndexBuffer);
dev.DrawIndexed(/*Primitive.TriangleList,*/ gridIndexBuffer.Capacity, 0, 0);
}
var rastState = Game.InputDevice.IsKeyDown(Keys.Tab) ? RasterizerState.Wireframe : RasterizerState.CullCW;
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_POLY | GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_TEXTURED | (Config.ShowFrames ? GlobeFlags.SHOW_FRAMES : GlobeFlags.NONE),
Primitive.TriangleList,
BlendState.AlphaBlend,
rastState,
DepthStencilState.Default);
foreach (var globeTile in tilesToRender)
{
var tex = Game.GetService <LayerService>().MapLayer.CurrentMapSource.GetTile(globeTile.Value.X, globeTile.Value.Y, globeTile.Value.Z).Tile;
dev.PixelShaderResources[0] = tex;
dev.SetupVertexInput(globeTile.Value.VertexBuf, globeTile.Value.IndexBuf);
dev.DrawIndexed(globeTile.Value.IndexBuf.Capacity, 0, 0);
}
dotsBuf.Data.View = viewMatrixFloat;
dotsBuf.Data.Proj = projMatrixFloat;
dotsBuf.UpdateCBuffer();
//// Draw simple railroads
//if (simpleRailroadsVB != null && Config.ShowRailroads) {
// Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
// GlobeFlags.DRAW_VERTEX_LINES | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_SEGMENTED_LINES,
// Primitive.LineList,
// BlendState.AlphaBlend,
// RasterizerState.CullNone,
// DepthStencilState.None);
//
// constBuffer.Data.Temp = new Vector4(1.0f, 0.0f, 0.0f, 0.0f);
// constBuffer.UpdateCBuffer();
//
// Game.GraphicsDevice.VertexShaderConstants[1] = dotsBuf;
// Game.GraphicsDevice.GeometryShaderConstants[1] = dotsBuf;
// Game.GraphicsDevice.GeometryShaderConstants[0] = constBuffer;
// Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
// Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
//
// Game.GraphicsDevice.PixelShaderResources[0] = railRoadsTex;
// Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearWrap;
//
// dev.SetupVertexInput(simpleRailroadsVB, null);
// dev.Draw(simpleRailroadsVB.Capacity, 0);
//}
//// Draw buildings
//if (contourBuildingsVB != null && Config.ShowBuildingsContours) {
//
// Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
// GlobeFlags.DRAW_POLY | GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_COLOR,
// Primitive.LineList,
// BlendState.AlphaBlend,
// RasterizerState.CullNone,
// DepthStencilState.None);
//
//
// constBuffer.Data.Temp = new Vector4(1.0f, 0.0f, 0.0f, 0.0f);
// constBuffer.UpdateCBuffer();
//
// Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
// Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
//
// dev.SetupVertexInput(contourBuildingsVB, null);
// dev.Draw(contourBuildingsVB.Capacity, 0);
//}
// Draw Lines
if (linesBatch.Count != 0 && Config.ShowRoads)
{
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_POLY | GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_COLOR,
Primitive.LineList,
BlendState.AlphaBlend,
RasterizerState.CullNone,
DepthStencilState.None);
constBuffer.Data.Temp = new Vector4(1.0f, 0.0f, 0.0f, 0.0f);
constBuffer.UpdateCBuffer();
Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
foreach (var vb in linesBatch)
{
dev.SetupVertexInput(vb, null);
dev.Draw(vb.Capacity, 0);
}
}
// Draw simple railroads
if (linesPolyBatch.Count != 0 && Config.ShowRoads)
{
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_VERTEX_LINES | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_LINES,
Primitive.LineList,
BlendState.AlphaBlend,
RasterizerState.CullNone,
DepthStencilState.None);
constBuffer.Data.Temp = new Vector4(1.0f, 0.0f, 0.0f, 0.0f);
constBuffer.UpdateCBuffer();
Game.GraphicsDevice.VertexShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[0] = constBuffer;
Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
foreach (var vb in linesPolyBatch)
{
dev.SetupVertexInput(vb, null);
dev.Draw(vb.Capacity, 0);
}
}
if (Config.ShowPOI)
{
dotsBuf.Data.TexWHST = new Vector4(geoObjects.Width, geoObjects.Height, 164.0f, 0.05f);
dotsBuf.UpdateCBuffer();
Game.GraphicsDevice.VertexShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[0] = constBuffer;
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_VERTEX_LINES | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_DOTS | GlobeFlags.DOTS_WORLDSPACE,
Primitive.PointList,
BlendState.AlphaBlend,
RasterizerState.CullNone,
DepthStencilState.None);
Game.GraphicsDevice.PixelShaderResources[0] = geoObjects;
Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearClamp;
dev.SetupVertexInput(dotsVB, null);
dev.Draw(dotsVB.Capacity - geoObjectStart, geoObjectStart);
}
// Draw people
if (Config.ShowPeople)
{
dotsBuf.Data.TexWHST = new Vector4(socioClasses.Width, socioClasses.Height, 64.0f, 0.025f);
dotsBuf.UpdateCBuffer();
Game.GraphicsDevice.VertexShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[0] = constBuffer;
Game.GraphicsDevice.PixelShaderResources[0] = socioClasses;
Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearClamp;
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_VERTEX_LINES | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_DOTS | GlobeFlags.DOTS_WORLDSPACE,
Primitive.PointList,
BlendState.AlphaBlend,
RasterizerState.CullNone,
DepthStencilState.None);
dev.SetupVertexInput(dotsVB, null);
dev.Draw(geoObjectStart - 1, 0);
}
//// Draw heatmap
//if (Config.ShowHeatMap && heatVB != null) {
// constBuffer.Data.Temp = new Vector4((float)Config.MaxHeatMapLevel, Config.HeatMapTransparency, HeatMapDim, 0);
// constBuffer.UpdateCBuffer();
//
// Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
// Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
//
// Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
// GlobeFlags.DRAW_POLY | GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_HEAT,
// Primitive.TriangleList,
// BlendState.AlphaBlend,
// RasterizerState.CullNone,
// DepthStencilState.None);
//
//
// Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearClamp;
// Game.GraphicsDevice.PixelShaderSamplers[1] = SamplerState.AnisotropicClamp;
//
// Game.GraphicsDevice.PixelShaderResources[0] = heatMap;
// Game.GraphicsDevice.PixelShaderResources[1] = heatMapPalette;
//
// Game.GraphicsDevice.SetupVertexInput(heatVB, heatIB);
// Game.GraphicsDevice.DrawIndexed(heatIB.Capacity, 0, 0);
//}
//// Draw infection map
//if (Config.ShowInfectHeatMap && heatVB != null) {
// constBuffer.Data.Temp = new Vector4((float)Config.MaxInfectLevel, 0.0f, HeatMapDim, 0);
// constBuffer.UpdateCBuffer();
//
// Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
// Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
//
// Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
// GlobeFlags.DRAW_POLY | GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_HEAT,
// Primitive.TriangleList,
// BlendState.AlphaBlend,
// RasterizerState.CullNone,
// DepthStencilState.None);
//
// Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearClamp;
// Game.GraphicsDevice.PixelShaderSamplers[1] = SamplerState.AnisotropicClamp;
//
// Game.GraphicsDevice.PixelShaderResources[0] = infectMap;
// Game.GraphicsDevice.PixelShaderResources[1] = heatMapPalette;
//
// Game.GraphicsDevice.SetupVertexInput(heatVB, heatIB);
// Game.GraphicsDevice.DrawIndexed(/*Primitive.TriangleList,*/ heatIB.Capacity, 0, 0);
//}
////Draw atmosphere
//if (Config.ShowAtmosphere && atmosVB != null) {
// constBuffer.Data.Temp = new Vector4(atmosTime, Config.ArrowsScale, 0.0f, 0);
// constBuffer.UpdateCBuffer();
//
// Game.GraphicsDevice.VertexShaderConstants[0] = constBuffer;
// Game.GraphicsDevice.PixelShaderConstants[0] = constBuffer;
//
//
// Game.GraphicsDevice.PixelShaderResources[0] = atmosTexture;
// Game.GraphicsDevice.PixelShaderResources[1] = heatMapPalette;
// Game.GraphicsDevice.PixelShaderResources[2] = atmosNextTexture;
// Game.GraphicsDevice.PixelShaderResources[3] = arrowTex;
//
//
// Game.GraphicsDevice.PixelShaderSamplers[0] = SamplerState.LinearClamp;
// Game.GraphicsDevice.PixelShaderSamplers[1] = SamplerState.AnisotropicClamp;
//
// Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
// GlobeFlags.DRAW_VERTEX_POLY | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_ATMOSPHERE,
// Primitive.TriangleList,
// BlendState.AlphaBlend,
// RasterizerState.CullNone,
// DepthStencilState.None);
//
// Game.GraphicsDevice.SetupVertexInput(atmosVB, atmosIB);
// Game.GraphicsDevice.DrawIndexed(atmosIB.Capacity, 0, 0);
//}
//Draw airlines
if (airLinesVB != null && Config.ShowAirLines)
{
constBuffer.Data.Temp = new Vector4(1.0f, 0.0f, 0.0f, 0.0f);
constBuffer.UpdateCBuffer();
Game.GraphicsDevice.VertexShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.GeometryShaderConstants[0] = constBuffer;
Game.GraphicsDevice.GeometryShaderConstants[1] = dotsBuf;
Game.GraphicsDevice.PipelineState = myMiniFactory.ProducePipelineState(
GlobeFlags.DRAW_VERTEX_LINES | GlobeFlags.USE_GEOCOORDS | GlobeFlags.VERTEX_SHADER | GlobeFlags.DRAW_ARCS,
Primitive.LineList,
BlendState.Additive,
RasterizerState.CullNone,
DepthStencilState.Readonly);
dev.SetupVertexInput(airLinesVB, null);
dev.Draw(/*Primitive.LineList,*/ airLinesVB.Capacity, 0);
}
DrawDebugLines();
//Game.GetService<DebugStrings>().Add("Cam pos : " + Game.GetService<Camera>().CameraMatrix.TranslationVector, Color.Red);
//Game.GetService<DebugStrings>().Add("Radius : " + Config.earthRadius, Color.Red);
//Game.GetService<DebugStrings>().Add("Level : " + CurrentLevel, Color.Green);
//Game.GetService<DebugStrings>().Add("CamDistance : " + Config.CameraDistance, Color.Green);
//Game.GetService<DebugStrings>().Add("Pitch : " + Pitch, Color.Green);
//Game.GetService<DebugStrings>().Add("Width : " + Game.GraphicsDevice.Viewport.Width, Color.Green);
Game.GetService <DebugStrings>().Add("Height : " + (Config.CameraDistance - Config.earthRadius) * 1000.0, Color.Green);
}
public static float Angle(DQuaternion left, DQuaternion right)
{
return (float)Math.Acos(Dot(left, right));
}
//Spherically interpolates between from and to by t.
public static DQuaternion Slerp(DQuaternion start, DQuaternion end, float amount)
{
float opposite;
float inverse;
float dot = Dot(start, end); // Dot product
DQuaternion result = Zero;
if (Math.Abs(dot) > 1.0f) // If abs is greater then 1
{ // This simple logic helps with the
inverse = 1.0f - amount; // look between slerp and lerp
opposite = amount * Math.Sign(dot); // This is default for lerp
}
else // else it will calculate a
{ // better inverse and opposite using
float acos = (float)Math.Acos(Math.Abs(dot)); // acos and inverse sin
float invSin = (float)(1.0 / Math.Sin(acos));
inverse = (float)Math.Sin((1.0f - amount) * acos) * invSin;
opposite = (float)Math.Sin(amount * acos) * invSin * Math.Sign(dot);
}
result.x = (inverse * start.x) + (opposite * end.x);
result.y = (inverse * start.y) + (opposite * end.y);
result.z = (inverse * start.z) + (opposite * end.z);
result.w = (inverse * start.w) + (opposite * end.w);
return result;
}
//Rotates a rotation from towards to.
//The from quaternion is rotated towards to by an angular step of maxDegreesDelta (but note that the rotation will not overshoot).
//Negative values of maxDegreesDelta will move away from to until the rotation is exactly the opposite direction.
public static DQuaternion RotateTowards(DQuaternion from, DQuaternion to, float maxDegreesDelta)
{
float angle = Angle(from, to);
if (angle < maxDegreesDelta)
return to;
else
return Slerp(from, to, maxDegreesDelta / angle);
}
//Interpolates between from and to by t and normalizes the result afterwards.
//This is faster than Slerp but looks worse if the rotations are far apart.
//Similar to Slerp but normalizes at the end. Probably why for the terrible looks
public static DQuaternion Lerp(DQuaternion start, DQuaternion end, float amount)
{
float inverse = 1.0f - amount;
DQuaternion result = Zero;
if (Dot(start, end) >= 0.0f)
{
result.x = (inverse * start.x) + (amount * end.x);
result.y = (inverse * start.y) + (amount * end.y);
result.z = (inverse * start.z) + (amount * end.z);
result.w = (inverse * start.w) + (amount * end.w);
}
else
{
result.x = (inverse * start.x) - (amount * end.x);
result.y = (inverse * start.y) - (amount * end.y);
result.z = (inverse * start.z) - (amount * end.z);
result.w = (inverse * start.w) - (amount * end.w);
}
result.Normalize();
return result;
}
double EnergyFromOrientation(DQuaternion o)
{
return(EnergyFromOrientation(o, AngularMomentum, Inertia));
}
void SetState(SurfaceCameraState state)
{
CameraPosition = FreeSurfacePosition = state.FreeSurfacePosition;
CameraDistance = CameraPosition.Length();
FreeSurfaceYaw = state.FreeSurfaceYaw;
FreeSurfacePitch = state.FreeSurfacePitch;
freeSurfaceRotation = state.FreeRotation;
var newLonLat = GetCameraLonLat();
Yaw = newLonLat.X;
Pitch = -newLonLat.Y;
}
public void Update(GameTime gameTime)
{
UpdateProjectionMatrix();
CameraPosition = DVector3.Transform(new DVector3(0, 0, CameraDistance), Rotation);
ViewMatrix = DMatrix.LookAtRH(CameraPosition, DVector3.Zero, DVector3.UnitY);
ViewMatrixWithTranslation = ViewMatrix;
ViewMatrix.TranslationVector = DVector3.Zero;
FinalCamPosition = CameraPosition;
if (CameraState == CameraStates.ViewToPoint) {
var mat = CalculateBasisOnSurface();
DVector3 lookAtPoint = mat.Up * EarthRadius;
double length = CameraDistance - EarthRadius;
var quat = DQuaternion.RotationAxis(DVector3.UnitY, ViewToPointYaw) * DQuaternion.RotationAxis(DVector3.UnitX, ViewToPointPitch);
var qRot = DMatrix.RotationQuaternion(quat);
var matrix = qRot * mat;
var pointOffset = DVector3.Transform(new DVector3(0, 0, length), matrix);
var camPoint = new DVector3(pointOffset.X, pointOffset.Y, pointOffset.Z) + lookAtPoint;
FinalCamPosition = camPoint;
ViewMatrix = DMatrix.LookAtRH(camPoint, lookAtPoint, mat.Up);
ViewMatrixWithTranslation = ViewMatrix;
ViewMatrix.TranslationVector = DVector3.Zero;
} else if (CameraState == CameraStates.FreeSurface) {
var mat = CalculateBasisOnSurface();
#region Input
// Update surface camera yaw and pitch
//if (input.IsKeyDown(Keys.RightButton)) {
// FreeSurfaceYaw += input.RelativeMouseOffset.X*0.0003;
// FreeSurfacePitch -= input.RelativeMouseOffset.Y*0.0003;
//
// input.IsMouseCentered = false;
// input.IsMouseHidden = true;
//}
//else {
// input.IsMouseCentered = false;
// input.IsMouseHidden = false;
//}
//if (Game.Keyboard.IsKeyDown(Input.Keys.Left)) FreeSurfaceYaw -= gameTime.ElapsedSec * 0.7;
//if (Game.Keyboard.IsKeyDown(Input.Keys.Right)) FreeSurfaceYaw += gameTime.ElapsedSec * 0.7;
//if (Game.Keyboard.IsKeyDown(Input.Keys.Up)) FreeSurfacePitch -= gameTime.ElapsedSec * 0.7;
//if (Game.Keyboard.IsKeyDown(Input.Keys.Down)) FreeSurfacePitch += gameTime.ElapsedSec * 0.7;
//FreeSurfaceYaw = DMathUtil.Clamp(FreeSurfaceYaw, -DMathUtil.PiOverTwo, DMathUtil.PiOverTwo);
if (FreeSurfaceYaw > DMathUtil.TwoPi) FreeSurfaceYaw -= DMathUtil.TwoPi;
if (FreeSurfaceYaw < -DMathUtil.TwoPi) FreeSurfaceYaw += DMathUtil.TwoPi;
// Calculate free cam rotation matrix
if (!freezeFreeCamRotation)
freeSurfaceRotation = DQuaternion.RotationAxis(DVector3.UnitY, FreeSurfaceYaw) * DQuaternion.RotationAxis(DVector3.UnitX, FreeSurfacePitch);
var quat = freeSurfaceRotation;
var qRot = DMatrix.RotationQuaternion(quat);
var matrix = qRot * mat;
var velDir = matrix.Forward * velocityDirection.X + mat.Up * velocityDirection.Y + matrix.Right * velocityDirection.Z;
if (velDir.Length() != 0) {
velDir.Normalize();
}
#endregion
double fac = ((CameraDistance - EarthRadius) - Parameters.MinDistVelocityThreshold) / (Parameters.MaxDistVelocityThreshold - Parameters.MinDistVelocityThreshold);
fac = DMathUtil.Clamp(fac, 0.0, 1.0);
FreeSurfaceVelocityMagnitude = DMathUtil.Lerp(Parameters.MinVelocityFreeSurfCam, Parameters.MaxVelocityFreeSurfCam, fac);
// Update camera position
FinalCamPosition = FreeSurfacePosition = FreeSurfacePosition + velDir * FreeSurfaceVelocityMagnitude * gameTime.ElapsedSec;
CameraPosition = FinalCamPosition;
velocityDirection = DVector3.Zero;
//Calculate view matrix
ViewMatrix = DMatrix.LookAtRH(FinalCamPosition, FinalCamPosition + matrix.Forward, matrix.Up);
ViewMatrixWithTranslation = ViewMatrix;
ViewMatrix.TranslationVector = DVector3.Zero;
// Calculate new yaw and pitch
CameraDistance = CameraPosition.Length();
var newLonLat = GetCameraLonLat();
Yaw = newLonLat.X;
Pitch = -newLonLat.Y;
}
ViewMatrixFloat = DMatrix.ToFloatMatrix(ViewMatrix);
ProjMatrixFloat = DMatrix.ToFloatMatrix(ProjMatrix);
//var viewDir = CameraPosition / CameraPosition.Length();
}
// Update is called once per frame
void Update()
{
transform.rotation = DQuaternion.ToUnity(Orientation);
}
public void PlayAnimation(GameTime gameTime)
{
if(cameraAnimTrackStates == null) return;
freezeFreeCamRotation = true;
var state = cameraAnimTrackStates[curStateInd];
curTime += 0.016f; //gameTime.ElapsedSec;
if (curTime < state.WaitTime || curStateInd >= cameraAnimTrackStates.Count - 1) {
SetState(state);
if (curStateInd >= cameraAnimTrackStates.Count - 1)
StopAnimation();
return;
}
float time = curTime - state.WaitTime;
float amount = time/state.TransitionTime;
float factor = MathUtil.SmoothStep(amount);
factor = MathUtil.Clamp(factor, 0.0f, 1.0f);
var nextState = cameraAnimTrackStates[curStateInd+1];
var curPos = DVector3.Lerp(state.FreeSurfacePosition, nextState.FreeSurfacePosition, factor);
var curFreeRot = DQuaternion.Slerp(state.FreeRotation, nextState.FreeRotation, factor);
freeSurfaceRotation = curFreeRot;
CameraPosition = FreeSurfacePosition = curPos;
var newLonLat = GetCameraLonLat();
Yaw = newLonLat.X;
Pitch = -newLonLat.Y;
if (curTime > state.WaitTime + state.TransitionTime) {
curStateInd++;
curTime = 0;
}
}
public static DQuaternion QuaternionFromTwoDirs(DVector3 UnitFrom, DVector3 UnitTo)
{
UnitFrom.Normalize();
UnitTo.Normalize();
DQuaternion quat;
double CosA = DVector3.Dot(UnitFrom, UnitTo);
if (CosA < -0.999999d)
{ // angle close to PI ( can replaced by Bisect.lensquared() < 0.000001f ) ;
DVector3 CrossVec = new DVector3(0d, UnitFrom.X, -UnitFrom.Y); // cross with (1, 0, 0)
if ((UnitFrom.Z * UnitFrom.Z) > (UnitFrom.Y * UnitFrom.Y))
{
// if (0, 1, 0) Cross > (1, 0, 0) Cross
CrossVec = new DVector3(-UnitFrom.Z, 0, UnitFrom.X); // cross with (0 ,1, 0)
}
CrossVec.Normalize();
quat = new DQuaternion(CrossVec.X, CrossVec.Y, CrossVec.Z, 0.0d);
}
else
{
DVector3 Bisect = DVector3.Add(UnitFrom, UnitTo);
Bisect.Normalize();
DVector3 BCross = DVector3.Cross(UnitFrom, Bisect);
quat = new DQuaternion(BCross.X, BCross.Y, BCross.Z, DVector3.Dot(UnitFrom, Bisect));
}
return quat;
}
void GetEulerAngles(DQuaternion q, out double yaw, out double pitch, out double roll)
{
double w2 = q.W*q.W;
double x2 = q.X*q.X;
double y2 = q.Y*q.Y;
double z2 = q.Z*q.Z;
double unitLength = w2 + x2 + y2 + z2; // Normalised == 1, otherwise correction divisor.
double abcd = q.W*q.X + q.Y*q.Z;
double eps = 1e-7; // TODO: pick from your math lib instead of hardcoding.
double pi = 3.14159265358979323846; // TODO: pick from your math lib instead of hardcoding.
if (abcd > (0.5-eps)*unitLength) {
yaw = 2 * Math.Atan2(q.Y, q.W);
pitch = pi;
roll = 0;
}
else if (abcd < (-0.5+eps)*unitLength) {
yaw = -2 * Math.Atan2(q.Y, q.W);
pitch = -pi;
roll = 0;
}
else {
double adbc = q.W*q.Z - q.X*q.Y;
double acbd = q.W*q.Y - q.X*q.Z;
yaw = Math.Atan2(2*adbc, 1 - 2*(z2+x2));
pitch = Math.Asin(2*abcd/unitLength);
roll = Math.Atan2(2 * acbd, 1 - 2 * (y2 + x2));
}
}
//The dot product between two rotations.
public static float Dot(DQuaternion left, DQuaternion right)
{
return (left.x * right.x) + (left.y * right.y) + (left.z * right.z) + (left.w * right.w);
}
