public void Execute(int i)
{
FractalPart parent = parents[i / 5];
FractalPart part = parts[i];
part.spinAngle += part.spinVelocity * deltaTime;
float3 upAxis = mul(mul(parent.worldRotation, part.rotation), up());
float3 sagAxis = cross(up(), upAxis);
float sagMagnitude = length(sagAxis);
quaternion baseRotation;
if (sagMagnitude > 0f)
{
sagAxis /= sagMagnitude;
quaternion sagRotation = quaternion.AxisAngle(sagAxis, part.maxSagAngle * sagMagnitude);
baseRotation = mul(sagRotation, parent.worldRotation);
}
else
{
baseRotation = parent.worldRotation;
}
part.worldRotation = mul(baseRotation,
mul(part.rotation, quaternion.RotateY(part.spinAngle))
);
part.worldPosition =
parent.worldPosition +
mul(part.worldRotation, float3(0f, 1.5f * scale, 0f));
parts[i] = part;
float3x3 r = float3x3(part.worldRotation) * scale;
matrices[i] = float3x4(r.c0, r.c1, r.c2, part.worldPosition);
}
public void TestAabb()
{
float3 v0 = float3(100, 200, 300);
float3 v1 = float3(200, 300, 400);
float3 v2 = float3(50, 100, 350);
Aabb a0; a0.Min = float3.zero; a0.Max = v0;
Aabb a1; a1.Min = float3.zero; a1.Max = v1;
Aabb a2; a2.Min = v2; a2.Max = v1;
Aabb a3; a3.Min = v2; a3.Max = v0;
Assert.IsTrue(a0.IsValid);
Assert.IsTrue(a1.IsValid);
Assert.IsTrue(a2.IsValid);
Assert.IsFalse(a3.IsValid);
Assert.IsTrue(a1.Contains(a0));
Assert.IsFalse(a0.Contains(a1));
Assert.IsTrue(a1.Contains(a2));
Assert.IsFalse(a2.Contains(a1));
Assert.IsFalse(a0.Contains(a2));
Assert.IsFalse(a2.Contains(a0));
// Test Expand / Contains
{
Aabb a5; a5.Min = v2; a5.Max = v1;
float3 testPoint = float3(v2.x - 1.0f, v1.y + 1.0f, .5f * (v2.z + v1.z));
Assert.IsFalse(a5.Contains(testPoint));
a5.Expand(1.5f);
Assert.IsTrue(a5.Contains(testPoint));
}
// Test transform
{
Aabb ut; ut.Min = v0; ut.Max = v1;
// Identity transform should not modify aabb
Aabb outAabb = Unity.Physics.Math.TransformAabb(RigidTransform.identity, ut);
TestUtils.AreEqual(ut.Min, outAabb.Min, 1e-3f);
// Test translation
outAabb = Unity.Physics.Math.TransformAabb(new RigidTransform(quaternion.identity, float3(100.0f, 0.0f, 0.0f)), ut);
Assert.AreEqual(outAabb.Min.x, 200);
Assert.AreEqual(outAabb.Min.y, 200);
Assert.AreEqual(outAabb.Max.x, 300);
Assert.AreEqual(outAabb.Max.z, 400);
// Test rotation
quaternion rot = quaternion.EulerXYZ(0.0f, 0.0f, k_pi2);
outAabb = Unity.Physics.Math.TransformAabb(new RigidTransform(rot, float3.zero), ut);
TestUtils.AreEqual(outAabb.Min, float3(-300.0f, 100.0f, 300.0f), 1e-3f);
TestUtils.AreEqual(outAabb.Max, float3(-200.0f, 200.0f, 400.0f), 1e-3f);
TestUtils.AreEqual(outAabb.SurfaceArea, ut.SurfaceArea, 1e-2f);
}
}
public void TestAabbTransform()
{
Random rnd = new Random(0x12345678);
for (int i = 0; i < 100; i++)
{
quaternion r = rnd.NextQuaternionRotation();
float3 t = rnd.NextFloat3();
Aabb orig = new Aabb();
orig.Include(rnd.NextFloat3());
orig.Include(rnd.NextFloat3());
Aabb outAabb1 = Unity.Physics.Math.TransformAabb(new RigidTransform(r, t), orig);
Physics.Math.MTransform bFromA = new Physics.Math.MTransform(r, t);
Aabb outAabb2 = Unity.Physics.Math.TransformAabb(bFromA, orig);
TestUtils.AreEqual(outAabb1.Min, outAabb2.Min, 1e-3f);
TestUtils.AreEqual(outAabb1.Max, outAabb2.Max, 1e-3f);
}
}
public static quaternion conjugate(quaternion q)
{
return(quaternion(q.value * float4(-1.0f, -1.0f, -1.0f, 1.0f)));
}
public RigidTransform(float4x4 transform)
{
this.rot = new quaternion(transform);
this.pos = transform.c3.xyz;
}
public static float3 rotate(quaternion q, float3 dir)
{
float3 t = 2 * cross(q.value.xyz, dir);
return(dir + q.value.w * t + cross(q.value.xyz, t));
}
public static float3 forward(quaternion q)
{
return(mul(q, float3(0, 0, 1)));
} // for compatibility
public static uint hash(quaternion q)
{
return(hash(q.value));
}
public static quaternion mul(quaternion a, quaternion b)
{
return(quaternion(a.value.wwww * b.value + (a.value.xyzx * b.value.wwwx + a.value.yzxy * b.value.zxyy) * float4(1.0f, 1.0f, 1.0f, -1.0f) - a.value.zxyz * b.value.yzxz));
}
public static float lengthsq(quaternion q)
{
return(dot(q.value, q.value));
}
public static float3 mul(quaternion q, float3 vector)
{
float3 t = 2 * cross(q.value.xyz, vector);
return(vector + q.value.w * t + cross(q.value.xyz, t));
}
public static quaternion inverse(quaternion q)
{
return(conjugate(normalize(q)));
}
public static quaternion conjugate(quaternion q)
{
return(quaternion(q.value * float4(-1.0f, -1.0f, -1.0f, 1.0f))); // TODO: should only be one xorps
}
public static float4x4 float4x4(quaternion rotation, float3 translation)
{
return(new float4x4(rotation, translation));
}
public static float3x3 float3x3(quaternion rotation)
{
return(new float3x3(rotation));
}
public static quaternion lerp(quaternion lhs, quaternion rhs, float t)
{
throw new System.NotImplementedException();
// var res = math.normalize(lhs.value + t * (math.chgsign(rhs.value, math.dot(lhs.value, rhs.value)) - rhs.value));
// return new quaternion(res);
}
//@TODO: Decide on saturate for t (old math lib did it...)
public static quaternion slerp(quaternion lhs, quaternion rhs, float t)
{
throw new System.NotImplementedException();
}
public static quaternion inverse(quaternion q)
{
float4 x = q.value;
return(quaternion(rcp(dot(x, x)) * x * float4(-1.0f, -1.0f, -1.0f, 1.0f)));
}
public static float dot(quaternion a, quaternion b)
{
return(dot(a.value, b.value));
}
public static float3 forward(quaternion q)
{
return(mul(q, float3(0, 0, 1)));
}
public static quaternion normalize(quaternion q)
{
float4 x = q.value;
return(quaternion(rsqrt(dot(x, x)) * x));
}
public static float3 up(quaternion q)
{
return(mul(q, float3(0, 1, 0)));
}
public static float3 rotate(quaternion q, float3 v)
{
float3 t = 2 * cross(q.value.xyz, v);
return(v + q.value.w * t + cross(q.value.xyz, t));
}
public RigidTransform(quaternion rotation, float3 translation)
{
this.rot = rotation;
this.pos = translation;
}
public static uint4 hashwide(quaternion q)
{
return(hashwide(q.value));
}
public static RigidTransform RigidTransform(quaternion rot, float3 pos)
{
return(new RigidTransform(rot, pos));
}
public RigidTransform(float3x3 rotation, float3 translation)
{
this.rot = new quaternion(rotation);
this.pos = translation;
}
public override void Update(float delta)
{
if (_active && !_exitingWormhole && !_enteringWormhole)
{
RecalculateThrust();
foreach (var thruster in _allThrusters)
{
thruster.Axis = 0;
}
var rightThrusterTorqueCompensation = abs(RightStrafeTotalTorque) / RightStrafeTorqueThrusters.Count;
foreach (var thruster in _rightThrusters)
{
var thrust = 0f;
thrust += MovementDirection.x;
if (RightStrafeTorqueThrusters.Contains(thruster))
{
thrust -= MovementDirection.x * (rightThrusterTorqueCompensation / (abs(thruster.Torque) * thruster.Thrust));
}
thruster.Axis = thrust;
}
var leftThrusterTorqueCompensation = abs(LeftStrafeTotalTorque) / LeftStrafeTorqueThrusters.Count;
foreach (var thruster in _leftThrusters)
{
var thrust = 0f;
thrust += -MovementDirection.x;
if (LeftStrafeTorqueThrusters.Contains(thruster))
{
thrust += MovementDirection.x * (leftThrusterTorqueCompensation / (abs(thruster.Torque) * thruster.Thrust));
}
thruster.Axis = thrust;
}
foreach (var thruster in _forwardThrusters)
{
thruster.Axis += MovementDirection.y;
}
foreach (var thruster in _reverseThrusters)
{
thruster.Axis += -MovementDirection.y;
}
var look = normalize(LookDirection.xz);
var deltaRot = dot(look, normalize(Direction).Rotate(ItemRotation.Clockwise));
if (abs(deltaRot) < .01f)
{
deltaRot = 0;
Direction = lerp(Direction, look, min(delta, 1));
}
deltaRot = pow(abs(deltaRot), .5f) * sign(deltaRot);
foreach (var thruster in _clockwiseThrusters)
{
thruster.Axis += deltaRot;
}
foreach (var thruster in _counterClockwiseThrusters)
{
thruster.Axis += -deltaRot;
}
foreach (var drive in _aetherDrives)
{
drive.Axis = float3(MovementDirection.y, MovementDirection.x, deltaRot);
}
}
var velocityMagnitude = length(Velocity);
if (velocityMagnitude > .01f)
{
Velocity = normalize(Velocity) * AetheriaMath.Decay(velocityMagnitude, HullData.Drag, delta);
}
Position.xz += Velocity * delta;
var normal = Zone.GetNormal(Position.xz);
var force = new float2(normal.x, normal.z);
var forceMagnitude = lengthsq(force);
if (forceMagnitude > .001f)
{
var fa = 1 / (1 - forceMagnitude) - 1;
Velocity += normalize(force) * Zone.Settings.GravityStrength * fa;
}
var shipRight = Direction.Rotate(ItemRotation.Clockwise);
var forward = cross(float3(shipRight.x, 0, shipRight.y), normal);
Rotation = quaternion.LookRotation(forward, normal);
base.Update(delta);
if (_exitingWormhole)
{
_wormholeAnimationProgress += delta / ItemManager.GameplaySettings.WormholeAnimationDuration;
if (_wormholeAnimationProgress < 1)
{
if (_wormholeAnimationProgress < ItemManager.GameplaySettings.WormholeExitCurveStart)
{
Position.xz = _wormholePosition;
Rotation = quaternion.LookRotation(float3(0, 1, 0), float3(-Direction.x, 0, -Direction.y));
}
else
{
var exitLerp = (_wormholeAnimationProgress - ItemManager.GameplaySettings.WormholeExitCurveStart) /
(1 - ItemManager.GameplaySettings.WormholeExitCurveStart);
exitLerp = AetheriaMath.Smootherstep(exitLerp); // Square the interpolation variable to produce curve with zero slope at start
Position.xz = _wormholePosition + normalize(_wormholeExitVelocity) * exitLerp * ItemManager.GameplaySettings.WormholeExitRadius;
Rotation = quaternion.LookRotation(
lerp(float3(0, 1, 0), forward, exitLerp),
lerp(float3(-Direction.x, 0, -Direction.y), normal, exitLerp));
}
Position.y = Position.y - lerp(ItemManager.GameplaySettings.WormholeDepth, 0, _wormholeAnimationProgress);
}
else
{
_exitingWormhole = false;
OnExitedWormhole?.Invoke();
OnExitedWormhole = null;
Velocity = _wormholeExitVelocity;
}
}
if (_enteringWormhole)
{
_wormholeAnimationProgress += delta / ItemManager.GameplaySettings.WormholeAnimationDuration;
if (_wormholeAnimationProgress < 1)
{
if (_wormholeAnimationProgress < 1 - ItemManager.GameplaySettings.WormholeExitCurveStart)
{
var enterLerp = _wormholeAnimationProgress / (1 - ItemManager.GameplaySettings.WormholeExitCurveStart);
enterLerp = AetheriaMath.Smootherstep(enterLerp); // Square the interpolation variable to produce curve with zero slope at vertical
Position.xz = lerp(_wormholeEntryPosition, _wormholePosition, enterLerp);
Rotation = quaternion.LookRotation(
lerp(forward, float3(0, -1, 0), enterLerp),
lerp(normal, float3(-_wormholeEntryDirection.x, 0, -_wormholeEntryDirection.y), enterLerp));
}
else
{
Position.xz = _wormholePosition;
Rotation = quaternion.LookRotation(float3(0, -1, 0),
float3(-_wormholeEntryDirection.x, 0, -_wormholeEntryDirection.y));
}
Position.y = Position.y - lerp(0, ItemManager.GameplaySettings.WormholeDepth, _wormholeAnimationProgress);
}
else
{
_enteringWormhole = false;
OnEnteredWormhole?.Invoke();
OnEnteredWormhole = null;
}
}
}
