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; } } }