From a64ff841ac0dde229f22b4228f8cf479fea3ebff Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 29 Jan 2026 09:04:51 +0100 Subject: [PATCH] Initial commit from NanoBrain-Unity --- Cluster.cs | 259 +++++++ Cluster.cs.meta | 2 + ClusterPrefab.cs | 84 ++ ClusterPrefab.cs.meta | 2 + Editor.meta | 8 + Editor/NeuroidWindow.cs | 302 ++++++++ Editor/NeuroidWindow.cs.meta | 2 + INucleus.cs | 54 ++ INucleus.cs.meta | 2 + Identity.asset | 60 ++ Identity.asset.meta | 8 + LinearAlgebra.meta | 8 + LinearAlgebra/.editorconfig | 19 + .../.gitea/workflows/unit_tests.yaml | 37 + LinearAlgebra/.gitignore | 5 + LinearAlgebra/LinearAlgebra-csharp.sln | 30 + LinearAlgebra/src/Angle.cs | 341 ++++++++ LinearAlgebra/src/Decomposition.cs | 287 +++++++ LinearAlgebra/src/Direction.cs | 261 +++++++ LinearAlgebra/src/Float.cs | 41 + LinearAlgebra/src/LinearAlgebra.csproj | 14 + LinearAlgebra/src/Matrix.cs | 689 +++++++++++++++++ LinearAlgebra/src/Quat32.cs | 87 +++ LinearAlgebra/src/Quaternion.cs | 582 ++++++++++++++ LinearAlgebra/src/Spherical.cs | 279 +++++++ LinearAlgebra/src/SwingTwist.cs | 136 ++++ LinearAlgebra/src/Vector2Float.cs | 479 ++++++++++++ LinearAlgebra/src/Vector2Int.cs | 185 +++++ LinearAlgebra/src/Vector3Float.cs | 402 ++++++++++ LinearAlgebra/src/Vector3Int.cs | 273 +++++++ LinearAlgebra/src/float16.cs | 322 ++++++++ LinearAlgebra/test/AngleTest.cs | 501 ++++++++++++ LinearAlgebra/test/DirectionTest.cs | 226 ++++++ LinearAlgebra/test/LinearAlgebra_Test.csproj | 19 + LinearAlgebra/test/QuaternionTest.cs | 185 +++++ LinearAlgebra/test/SphericalTest.cs | 271 +++++++ LinearAlgebra/test/SwingTwistTest.cs | 131 ++++ LinearAlgebra/test/Vector2FloatTest.cs | 364 +++++++++ LinearAlgebra/test/Vector2IntTest.cs | 270 +++++++ LinearAlgebra/test/Vector3FloatTest.cs | 581 ++++++++++++++ LinearAlgebra/test/Vector3IntTest.cs | 349 +++++++++ MemoryCell.cs | 65 ++ MemoryCell.cs.meta | 2 + NanoBrain-Unity.code-workspace | 12 + NanoBrain-Unity.code-workspace.meta | 7 + Neuroid.cs | 82 ++ Neuroid.cs.meta | 2 + Neuron.cs | 321 ++++++++ Neuron.cs.meta | 2 + NucleusArray.cs | 67 ++ NucleusArray.cs.meta | 2 + Perceptoid.cs | 105 +++ Perceptoid.cs.meta | 2 + Receptor.cs | 175 +++++ Receptor.cs.meta | 2 + Scene.meta | 8 + Scene/TestScene.unity | 487 ++++++++++++ Scene/TestScene.unity.meta | 7 + Synapse.cs | 22 + Synapse.cs.meta | 2 + Velocity.asset | 108 +++ Velocity.asset.meta | 8 + VisualEditor.meta | 8 + VisualEditor/Editor.meta | 8 + VisualEditor/Editor/BrainPickerWindow.cs | 66 ++ VisualEditor/Editor/BrainPickerWindow.cs.meta | 2 + VisualEditor/Editor/ClusterInspector.cs | 728 ++++++++++++++++++ VisualEditor/Editor/ClusterInspector.cs.meta | 2 + .../Editor/NanoBrainComponent_Editor.cs | 129 ++++ .../Editor/NanoBrainComponent_Editor.cs.meta | 2 + VisualEditor/Editor/NanoBrainEditor.cs | 511 ++++++++++++ VisualEditor/Editor/NanoBrainEditor.cs.meta | 2 + VisualEditor/Editor/NanoBrainInspector.cs | 645 ++++++++++++++++ .../Editor/NanoBrainInspector.cs.meta | 2 + VisualEditor/NanoBrain.cs | 102 +++ VisualEditor/NanoBrain.cs.meta | 2 + VisualEditor/NanoBrainComponent.cs | 35 + VisualEditor/NanoBrainComponent.cs.meta | 2 + VisualEditor/Resources.meta | 8 + VisualEditor/Resources/GraphStyles.uss | 12 + VisualEditor/Resources/GraphStyles.uss.meta | 11 + 81 files changed, 11922 insertions(+) create mode 100644 Cluster.cs create mode 100644 Cluster.cs.meta create mode 100644 ClusterPrefab.cs create mode 100644 ClusterPrefab.cs.meta create mode 100644 Editor.meta create mode 100644 Editor/NeuroidWindow.cs create mode 100644 Editor/NeuroidWindow.cs.meta create mode 100644 INucleus.cs create mode 100644 INucleus.cs.meta create mode 100644 Identity.asset create mode 100644 Identity.asset.meta create mode 100644 LinearAlgebra.meta create mode 100644 LinearAlgebra/.editorconfig create mode 100644 LinearAlgebra/.gitea/workflows/unit_tests.yaml create mode 100644 LinearAlgebra/.gitignore create mode 100644 LinearAlgebra/LinearAlgebra-csharp.sln create mode 100644 LinearAlgebra/src/Angle.cs create mode 100644 LinearAlgebra/src/Decomposition.cs create mode 100644 LinearAlgebra/src/Direction.cs create mode 100644 LinearAlgebra/src/Float.cs create mode 100644 LinearAlgebra/src/LinearAlgebra.csproj create mode 100644 LinearAlgebra/src/Matrix.cs create mode 100644 LinearAlgebra/src/Quat32.cs create mode 100644 LinearAlgebra/src/Quaternion.cs create mode 100644 LinearAlgebra/src/Spherical.cs create mode 100644 LinearAlgebra/src/SwingTwist.cs create mode 100644 LinearAlgebra/src/Vector2Float.cs create mode 100644 LinearAlgebra/src/Vector2Int.cs create mode 100644 LinearAlgebra/src/Vector3Float.cs create mode 100644 LinearAlgebra/src/Vector3Int.cs create mode 100644 LinearAlgebra/src/float16.cs create mode 100644 LinearAlgebra/test/AngleTest.cs create mode 100644 LinearAlgebra/test/DirectionTest.cs create mode 100644 LinearAlgebra/test/LinearAlgebra_Test.csproj create mode 100644 LinearAlgebra/test/QuaternionTest.cs create mode 100644 LinearAlgebra/test/SphericalTest.cs create mode 100644 LinearAlgebra/test/SwingTwistTest.cs create mode 100644 LinearAlgebra/test/Vector2FloatTest.cs create mode 100644 LinearAlgebra/test/Vector2IntTest.cs create mode 100644 LinearAlgebra/test/Vector3FloatTest.cs create mode 100644 LinearAlgebra/test/Vector3IntTest.cs create mode 100644 MemoryCell.cs create mode 100644 MemoryCell.cs.meta create mode 100644 NanoBrain-Unity.code-workspace create mode 100644 NanoBrain-Unity.code-workspace.meta create mode 100644 Neuroid.cs create mode 100644 Neuroid.cs.meta create mode 100644 Neuron.cs create mode 100644 Neuron.cs.meta create mode 100644 NucleusArray.cs create mode 100644 NucleusArray.cs.meta create mode 100644 Perceptoid.cs create mode 100644 Perceptoid.cs.meta create mode 100644 Receptor.cs create mode 100644 Receptor.cs.meta create mode 100644 Scene.meta create mode 100644 Scene/TestScene.unity create mode 100644 Scene/TestScene.unity.meta create mode 100644 Synapse.cs create mode 100644 Synapse.cs.meta create mode 100644 Velocity.asset create mode 100644 Velocity.asset.meta create mode 100644 VisualEditor.meta create mode 100644 VisualEditor/Editor.meta create mode 100644 VisualEditor/Editor/BrainPickerWindow.cs create mode 100644 VisualEditor/Editor/BrainPickerWindow.cs.meta create mode 100644 VisualEditor/Editor/ClusterInspector.cs create mode 100644 VisualEditor/Editor/ClusterInspector.cs.meta create mode 100644 VisualEditor/Editor/NanoBrainComponent_Editor.cs create mode 100644 VisualEditor/Editor/NanoBrainComponent_Editor.cs.meta create mode 100644 VisualEditor/Editor/NanoBrainEditor.cs create mode 100644 VisualEditor/Editor/NanoBrainEditor.cs.meta create mode 100644 VisualEditor/Editor/NanoBrainInspector.cs create mode 100644 VisualEditor/Editor/NanoBrainInspector.cs.meta create mode 100644 VisualEditor/NanoBrain.cs create mode 100644 VisualEditor/NanoBrain.cs.meta create mode 100644 VisualEditor/NanoBrainComponent.cs create mode 100644 VisualEditor/NanoBrainComponent.cs.meta create mode 100644 VisualEditor/Resources.meta create mode 100644 VisualEditor/Resources/GraphStyles.uss create mode 100644 VisualEditor/Resources/GraphStyles.uss.meta diff --git a/Cluster.cs b/Cluster.cs new file mode 100644 index 0000000..44f68ed --- /dev/null +++ b/Cluster.cs @@ -0,0 +1,259 @@ +using System; +using System.Collections.Generic; +using UnityEngine; +using Unity.Mathematics; +using static Unity.Mathematics.math; + +[System.Serializable] +public class Cluster : INucleus { + // The ScriptableObject asset from which the runtime object has been created + public readonly ClusterPrefab prefab; + + public ClusterPrefab cluster { get; set; } + + [SerializeField] + protected string _name; + public virtual string name { + get => _name; + set => _name = value; + } + + // Hmm, a cluster instance can never be part of a scriptable object...(Cluster) + public Cluster(ClusterPrefab parent, ClusterPrefab prefab) { + this.prefab = prefab; + this.cluster = parent; + if (this.cluster != null) + this.cluster.nuclei.Add(this); + + // foreach (IReceptor nucleus in this.prefab.nuclei) { + // IReceptor clone = nucleus.CloneTo(null); + // this.dynamicNuclei.Add(clone); + // } + } + + public virtual IReceptor Clone() { + Neuron clone = new(this.cluster, this.name) { + array = this.array, + }; + + foreach (Synapse synapse in this.synapses) { + Synapse clonedSynapse = clone.AddSynapse(synapse.nucleus); + clonedSynapse.weight = synapse.weight; + } + foreach (INucleus receiver in this.receivers) { + clone.AddReceiver(receiver); + } + return clone; + } + + // Not sure if this belongs here... + [SerializeReference] + private NucleusArray _array; + public NucleusArray array { + get { return _array; } + set { _array = value; } + } + + #region Synapses + + // class ClusterSynapse : Synapse { + // public IReceptor receptor; + // public ClusterSynapse(IReceptor nucleus, INucleus receptor, float weight = 1.0f) : base(nucleus, weight) { + // this.receptor = receptor; + // } + // } + [SerializeField] + private List _synapses = new(); + public List synapses => _synapses; + + public Synapse AddSynapse(IReceptor sendingNucleus) { + Synapse synapse = new(sendingNucleus); //, this.prefab.inputs[0]); + this._synapses.Add(synapse); + return synapse; + // else { + // INucleus receptor = (INucleus)this.prefab.nuclei.Find(nucleus => nucleus is INucleus n && nucleus.name == nucleusName); + // ClusterSynapse synapse = new(sendingNucleus, receptor); + // receptor.AddSynapse(sendingNucleus); + // } + // // Add synapse to which neuron? + // return null; + } + + // Does this even exist already? + public void RemoveSynapse() { + + } + + #endregion Synapses + + #region Receivers + + [SerializeReference] + private List _receivers = new(); + public List receivers { + get { return _receivers; } + set { _receivers = value; } + } + + public virtual void AddReceiver(INucleus receivingNucleus) { + this._receivers.Add(receivingNucleus); + receivingNucleus.AddSynapse(this); + } + + public void RemoveReceiver(INucleus receiverNucleus) { + this._receivers.RemoveAll(receiver => receiver == receiverNucleus); + receiverNucleus.synapses.RemoveAll(synapse => synapse.nucleus == this); + } + + // public void AddReceiver(INucleus receivingNucleus) { + // int newLength = this._receivers.Count + 1; + // INucleus[] newReceivers = new INucleus[newLength]; + + // // Copy the existing receivers + // for (int ix = 0; ix < this._receivers.Count; ix++) + // newReceivers[ix] = this._receivers[ix]; + // // Add the new receivers + // newReceivers[this._receivers.Count] = receivingNucleus; + // // Replace the receivers with the new receivers + // this._receivers = new(newReceivers); + + // receivingNucleus.AddSynapse(this); + // } + + // public void RemoveReceiver(INucleus receivingNucleus) { + // Debug.Log("Clusterinstance. remote receiver"); + // int newLength = this._receivers.Count - 1; + // if (newLength < 0) + // // Array was empty, so we cannot remove anything + // return; + + // INucleus[] newReceivers = new INucleus[newLength]; + + // int newIx = 0; + // // Copy all receivers except receivingNucleus + // for (int ix = 0; ix < this._receivers.Count; ix++) { + // if (this._receivers[ix] == receivingNucleus) + // // skip the receiver we want to remote + // continue; + + // if (newIx >= newLength) + // // We want to copy more elements than expected + // // the receivingNucleus is not found + // // and the original array is returned + // return; + // newReceivers[newIx] = this._receivers[ix]; + // newIx++; + // } + // this._receivers = new(newReceivers); + // } + + #endregion Receivers + + #region Runtime + + [NonSerialized] + private int stale = 1000; + public bool isSleeping => lengthsq(this.outputValue) == 0; + + [NonSerialized] + protected float3 _outputValue; + public virtual float3 outputValue { + get { return _outputValue; } + set { + this.stale = 0; + _outputValue = value; + } + } + + #region Update + + public virtual void UpdateState() { + UpdateState(new float3(0, 0, 0)); + } + + public void UpdateState(float3 inputValue) { + float3 sum = inputValue; // new(0, 0, 0); + + //Applying the weight factgors + foreach (Synapse synapse in this.synapses) { + sum += synapse.weight * synapse.nucleus.outputValue; + } + + // This does not work because the prefab nucleus does not have a state + this.prefab.inputs[0].UpdateState(sum); + } + + public void UpdateNuclei() { + this.stale++; + if (this.stale > 2) + _outputValue = Vector3.zero; + + foreach (IReceptor nucleus in this.prefab.nuclei) + nucleus.UpdateNuclei(); + } + + #endregion Update + + #endregion Runtime + + /* + [SerializeField] + private List _dynamicNuclei; + public List dynamicNuclei {// = new(); + get { + if (_dynamicNuclei == null) { + this._dynamicNuclei = new(); + foreach (IReceptor nucleus in this.prefab.nuclei) { + IReceptor clone = nucleus.CloneTo(null); + this._dynamicNuclei.Add(clone); + } + } + return this._dynamicNuclei; + } + } + + public List _inputs = null; + public List inputs { + get { + this._inputs = new(); + if (this.dynamicNuclei != null) { + foreach (IReceptor receptor in this.dynamicNuclei) { + if (receptor is INucleus nucleus) + this._inputs.Add(nucleus); + } + } + return this._inputs; + } + } + + public INucleus output => this.dynamicNuclei[0] as INucleus; + + public float3 outputValue => this.output.outputValue; + + + public IReceptor CloneTo(ClusterPrefab parent) { + Cluster clone = new(parent, this.prefab); + return clone; + } + public IReceptor Clone() { + Cluster clone = new(this.cluster, this.prefab); + return clone; + } + + #region Properties + + public string name { + get { return prefab.name; } + set { prefab.name = value; } + } + + public bool isSleeping => lengthsq(this.outputValue) == 0; + + public NucleusArray array { get; set; } + + #endregion Properties + + + */ + +} diff --git a/Cluster.cs.meta b/Cluster.cs.meta new file mode 100644 index 0000000..a10caff --- /dev/null +++ b/Cluster.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: f13cdc4a175a9f379a00317ae68d8bea \ No newline at end of file diff --git a/ClusterPrefab.cs b/ClusterPrefab.cs new file mode 100644 index 0000000..db65b70 --- /dev/null +++ b/ClusterPrefab.cs @@ -0,0 +1,84 @@ +using System.Collections.Generic; +using UnityEngine; + +[CreateAssetMenu(menuName = "Passer/Cluster")] +public class ClusterPrefab : ScriptableObject { + + //public virtual Cluster cluster {get;set;} + + // The ScriptableObject asset from which the runtime object has been created + //public Cluster asset; + + [SerializeReference] + public List nuclei = new(); + + // public List subClusters = new(); + // public void AddSubCluster(ClusterInstance subCluster) { + // this.nuclei.Add(subCluster); + // } + + public virtual INucleus output => this.nuclei[0] as INucleus; + + public List _inputs = null; + public virtual List inputs { + get { + if (this._inputs == null) { + this._inputs = new(); + foreach (IReceptor receptor in this.nuclei) { + if (receptor is INucleus nucleus) + this._inputs.Add(nucleus); + } + } + return this._inputs; + } + } + + // Call this function to ensure that there is at least one nucleus + // This is an invariant and should be ensured before the nucleus is used + // because output requires it. + public void EnsureInitialization() { + nuclei ??= new List(); + if (nuclei.Count == 0) + new Neuron(this, "Output"); // Every cluster should have at least 1 neuron + } + + public void GarbageCollection() { + HashSet visitedNuclei = new(); + MarkNuclei(visitedNuclei, this.output); + //Debug.Log($"Garbage collection found {visitedNuclei.Count} Nuclei"); + this.nuclei.RemoveAll(nucleus => nucleus is INucleus n && visitedNuclei.Contains(n) == false); + } + + public void MarkNuclei(HashSet visitedNuclei, INucleus nucleus) { + if (nucleus is null) + return; + + visitedNuclei.Add(nucleus); + if (nucleus.synapses != null) { + HashSet visitedSynapses = new(); + foreach (Synapse synapse in nucleus.synapses) { + if (synapse != null && synapse.nucleus != null) { + visitedSynapses.Add(synapse); + if (synapse.nucleus is INucleus synapse_nucleus) + MarkNuclei(visitedNuclei, synapse_nucleus); + } + } + nucleus.synapses.RemoveAll(synapse => visitedSynapses.Contains(synapse) == false); + } + if (nucleus.receivers != null) { + HashSet visitedReceivers = new(); + foreach (INucleus receiver in nucleus.receivers) { + if (receiver != null && receiver != null) { + visitedReceivers.Add(receiver); + visitedNuclei.Add(receiver); + } + } + nucleus.receivers.RemoveAll(receiver => visitedReceivers.Contains(receiver) == false); + } + } + + public virtual void UpdateNuclei() { + foreach (IReceptor nucleus in this.nuclei) + nucleus.UpdateNuclei(); + } +} \ No newline at end of file diff --git a/ClusterPrefab.cs.meta b/ClusterPrefab.cs.meta new file mode 100644 index 0000000..ee35e0b --- /dev/null +++ b/ClusterPrefab.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 60a957541c24c57e78018c202ebb1d9b \ No newline at end of file diff --git a/Editor.meta b/Editor.meta new file mode 100644 index 0000000..090b3ac --- /dev/null +++ b/Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3aedf57a50b6dfa46a59457c87b8ef9d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Editor/NeuroidWindow.cs b/Editor/NeuroidWindow.cs new file mode 100644 index 0000000..187df35 --- /dev/null +++ b/Editor/NeuroidWindow.cs @@ -0,0 +1,302 @@ +/* +using UnityEditor; +using UnityEngine; +using System.Linq; +using System.Collections.Generic; + +public class NeuroidLayer { + public int ix = 0; + public List neuroids = new(); +} + +public class GraphEditorWindow : EditorWindow { + private Nucleus currentNucleus; + private List allNeuroids; + private Dictionary neuroidPositions = new(); + + private List layers = new(); + + private void OnEnable() { + EditorApplication.update += EditorUpdate; + Selection.selectionChanged += OnSelectionChange; + SelectNeuron(); + } + + private void AddToLayer(NeuroidLayer layer, Nucleus nucleus) { + layer.neuroids.Add(nucleus); + nucleus.layerIx = layer.ix; + // Store its position + Vector2Int neuroidPosition = new(layer.ix, layer.neuroids.Count - 1); + neuroidPositions[nucleus] = neuroidPosition; + + } + + private void BuildLayers() { + // A temporary list to track what's been added to layers + this.layers = new(); + int layerIx = 0; + + Nucleus selectedNucleus = this.currentNucleus; + if (selectedNucleus == null) + return; + NeuroidLayer currentLayer = new() { ix = layerIx }; + + //foreach (Nucleus outputNeuroid in selectedNucleus.receivers) { + foreach (Receiver receiver in selectedNucleus.receivers) { + Nucleus outputNeuroid = receiver.nucleus; + if (outputNeuroid != null) { + AddToLayer(currentLayer, outputNeuroid); + Debug.Log($"layer {layerIx} nucleus {outputNeuroid.name}"); + } + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + layerIx++; + currentLayer = new() { ix = layerIx }; + } + + AddToLayer(currentLayer, selectedNucleus); + this.layers.Add(currentLayer); + Debug.Log($"layer {layerIx} nucleus {selectedNucleus.name}"); + + layerIx++; + currentLayer = new() { ix = layerIx }; + + int six = 0; + // foreach (Synapse synapse in selectedNucleus.synapses.Values) { + // Debug.Log($"Synapse {six}"); + // Nucleus input = synapse.neuroid; + //foreach ((Nucleus input, Synapse synapse) in selectedNucleus.synapses) { + //foreach ((Nucleus input, float weight) in selectedNucleus.synapses) { + foreach (Synapse synapse in selectedNucleus.synapses) { + Nucleus input = synapse.nucleus; + if (input != null) { + AddToLayer(currentLayer, input); + Debug.Log($"layer {layerIx} nucleus {input.name}"); + } + six++; + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + } + } + + private void BuildLayers_old(List neuroids) { + if (neuroids == null) + return; + + // A temporary list to track what's been added to layers + this.layers = new(); + HashSet neuronVisited = new(); + int layerIx = 0; + + // While there are unvisited neuroid + while (neuroids.Any(neuroid => !neuronVisited.Contains(neuroid))) { + // Create the next layer + NeuroidLayer currentLayer = new() { ix = layerIx }; + int neuroidIx = 0; + + foreach (Neuroid neuroid in neuroids) { + // Skip neurons we already processed + if (neuronVisited.Contains(neuroid)) + continue; + + // if (neuroid.IsStale()) { + // Debug.Log($"neuron {neuroid.name} is stale {neuroid.stale}"); + // neuronVisited.Add(neuroid); + // continue; + // } + + // If the output neuroid is visited + // Note: this does not yet work for multiple outputs yet (see the use of First()) + // if (neuroid.receivers.Count == 0 // make sure the root neuroids are processed directly + // || (neuronVisited.Contains(neuroid.receivers.First()) && neuroid.receivers.First().layerIx == layerIx - 1)) { + if (neuroid.receivers.Count == 0 // make sure the root neuroids are processed directly + || (neuronVisited.Contains(neuroid.receivers.First().nucleus) && neuroid.receivers.First().nucleus.layerIx == layerIx - 1)) { + // Add it to the next layer + currentLayer.neuroids.Add(neuroid); + neuroid.layerIx = layerIx; + // Register it as visited + neuronVisited.Add(neuroid); + // Store its position + Vector2Int neuroidPosition = new(layerIx, neuroidIx); + neuroidPositions[neuroid] = neuroidPosition; + neuroidIx++; + Debug.Log($"Layer {layerIx} neuron {neuroidIx} name {neuroid.name}"); + } + } + + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + layerIx++; + } + } + } + + private void OnDisable() { + EditorApplication.update -= EditorUpdate; + Selection.selectionChanged -= OnSelectionChange; + } + + private void OnSelectionChange() { + SelectNeuron(); + Repaint(); + } + + private void EditorUpdate() { + if (EditorApplication.isPlaying) + Repaint(); + } + + private void OnGUI() { + GUILayout.Label("Graph Visualizer", EditorStyles.boldLabel); + + DrawGraph(); + } + + private void DrawGraph() { + if (currentNucleus == null) + return; + + foreach (NeuroidLayer layer in layers) + DrawLayer(layer); + } + + private void DrawLayer(NeuroidLayer layer) { + int column = layer.ix * 100; + int nodeCount = layer.neuroids.Count; + float maxValue = 0; + foreach (Nucleus nucleus in layer.neuroids) { + if (nucleus is Neuroid neuroid) { + float value = neuroid.outputValue.magnitude; + if (value > maxValue) + maxValue = value; + } + } + float spacing = 400f / nodeCount; + float margin = 100 + spacing / 2; + foreach (Nucleus layerNucleus in layer.neuroids) { + if (layerNucleus is Neuroid layerNeuroid) { + Vector2Int layerNeuroidPos = this.neuroidPositions[layerNeuroid]; + Vector3 parentPos = new(100 + layerNeuroidPos.x * 100, margin + layerNeuroidPos.y * spacing, 0.1f); + + int i = 0; + float inputSpacing = 400f / layerNeuroid.synapses.Count; + float inputMargin = 100 + inputSpacing / 2; + // foreach (Synapse synapse in layerNeuroid.synapses.Values) { + // if (synapse.neuroid != null) { + // if (this.neuroidPositions.ContainsKey(synapse.neuroid)) { + + // Vector2Int inputNeuroidPos = this.neuroidPositions[synapse.neuroid]; + //foreach ((Nucleus neuroid, Synapse synapse) in layerNeuroid.synapses) { + //foreach ((Nucleus neuroid, float weight) in layerNeuroid.synapses) { + foreach (Synapse synapse in layerNeuroid.synapses) { + Nucleus neuroid = synapse.nucleus; + float weight = synapse.weight; + if (neuroid != null) { + if (this.neuroidPositions.ContainsKey(neuroid)) { + Vector2Int inputNeuroidPos = this.neuroidPositions[neuroid]; + if (inputNeuroidPos.x == layerNeuroidPos.x + 1) { + Vector3 pos = new(100 + inputNeuroidPos.x * 100, inputMargin + inputNeuroidPos.y * inputSpacing, 0.0f); + + //float brightness = synapse.weight / 10.0f; + float brightness = weight / 10.0f; + Handles.color = new Color(brightness, brightness, brightness); + Handles.DrawLine(parentPos, pos); + } + } + } + } + + float size = 20; + if (layerNeuroid.isSleeping) + Handles.color = Color.black; + else { + float brightness = layerNeuroid.outputValue.magnitude / maxValue; + Handles.color = new Color(brightness, brightness, brightness); + } + Handles.DrawSolidDisc(parentPos, Vector3.forward, size); + Vector3 labelPos = parentPos - Vector3.down * (size + 0.2f); // below disc along up axis + GUIStyle style = new GUIStyle(EditorStyles.label) { + alignment = TextAnchor.UpperCenter, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold + }; + Handles.Label(labelPos, layerNeuroid.name, style); + + Rect neuronRect = new(parentPos.x - size, parentPos.y - size, size * 2, size * 2); + Event e = Event.current; + if (e != null && neuronRect.Contains(e.mousePosition)) { + HandleMouseHover(layerNeuroid, neuronRect); + // Process click + if (e.type == EventType.MouseDown && e.button == 0) { + // Consume the event so the scene doesn't also handle it + e.Use(); + HandleDiscClicked(layerNeuroid); + } + } + i++; + } + } + } + + private void HandleMouseHover(Neuroid neuroid, Rect rect) { + GUIContent tooltip; + // if (neuroid is SensoryNeuroid sensoryNeuroid) { + // tooltip = new( + // $"{sensoryNeuroid.name}" + + // $"\nThing {sensoryNeuroid.receptor.thingType}" + + // $"\nValue: {neuroid.outputValue}"); + // } + // else { + tooltip = new( + $"{neuroid.name}" + + $"\nsynapse count {neuroid.synapses.Count}" + + $"\nValue: {neuroid.outputValue}"); + // } + + Vector2 mousePosition = Event.current.mousePosition; + + // Display tooltip with some offset + Vector2 tooltipSize = GUI.skin.box.CalcSize(tooltip); + Rect tooltipRect = new Rect(mousePosition.x + 10, mousePosition.y + 10, tooltipSize.x, tooltipSize.y); + + GUI.Box(tooltipRect, tooltip); + } + + private void HandleDiscClicked(Nucleus nucleus) { + this.currentNucleus = nucleus; + BuildLayers(); + } + + // Update node colors based on selected GameObjects + private void SelectNeuron() { + GameObject[] selectedObjects = Selection.gameObjects; + if (selectedObjects.Length == 0) + return; + + GameObject selectedObject = selectedObjects[0]; + Boid boid = selectedObject.GetComponent(); + if (boid == null) + return; + + // Nucleus neuroid = boid.behaviour; + // this.currentNucleus = neuroid; + // if (neuroid == null) + // this.allNeuroids = new(); + // else + // this.allNeuroids = neuroid.brain.neuroids; + + + // Debug.Log($"Neuroncount = {this.allNeuroids.Count}"); + // BuildLayers(); + // Debug.Log($"Layercount = {this.layers.Count}"); + + } + + [MenuItem("Window/Neuroid Visualizer")] + public static void ShowWindow() { + GetWindow("Neuroid Visualizer"); + } +} +*/ \ No newline at end of file diff --git a/Editor/NeuroidWindow.cs.meta b/Editor/NeuroidWindow.cs.meta new file mode 100644 index 0000000..a8a1aa1 --- /dev/null +++ b/Editor/NeuroidWindow.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 26e68838038ea5243ae57bc81f4db8a8 \ No newline at end of file diff --git a/INucleus.cs b/INucleus.cs new file mode 100644 index 0000000..f7272f3 --- /dev/null +++ b/INucleus.cs @@ -0,0 +1,54 @@ +using System.Collections.Generic; +using Unity.Mathematics; + +public interface INucleus : IReceptor { + + #region static struct + + // Cluster + public ClusterPrefab cluster { get; } + + // Senders + public List synapses { get; } + public Synapse AddSynapse(IReceptor sender); + + public NucleusArray array { get; set; } + + #endregion static struct + + #region dynamic state + + public void UpdateState(); + public void UpdateState(float3 inputValue); + + + #endregion dynamic state +} + +public interface IReceptor { + #region static + + public string name { get; set; } + + // Receivers + public List receivers { get; set; } + + public void AddReceiver(INucleus receiver); + public void RemoveReceiver(INucleus receiverNucleus); + + #endregion static + + #region dynamic + + // float3 to prepare for SIMD + public float3 outputValue { get; } + + public void UpdateNuclei(); + public bool isSleeping { get; } + + #endregion dynamic + + //public IReceptor CloneTo(ClusterPrefab parent); + public IReceptor Clone(); +} + diff --git a/INucleus.cs.meta b/INucleus.cs.meta new file mode 100644 index 0000000..aed95bb --- /dev/null +++ b/INucleus.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 6a8a0e8965cea660abff254cab8a4723 \ No newline at end of file diff --git a/Identity.asset b/Identity.asset new file mode 100644 index 0000000..076c284 --- /dev/null +++ b/Identity.asset @@ -0,0 +1,60 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!114 &11400000 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 0} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 60a957541c24c57e78018c202ebb1d9b, type: 3} + m_Name: Identity + m_EditorClassIdentifier: Assembly-CSharp::Cluster + nuclei: + - rid: 2243601383627161705 + references: + version: 2 + RefIds: + - rid: 2243601383627161705 + type: {class: Neuron, ns: , asm: Assembly-CSharp} + data: + _name: Output + _synapses: [] + _receivers: [] + _array: + rid: 2243601383627161706 + _curvePreset: 0 + curve: + serializedVersion: 2 + m_Curve: + - serializedVersion: 3 + time: 0 + value: 0 + inSlope: 0 + outSlope: 1 + tangentMode: 0 + weightedMode: 0 + inWeight: 0 + outWeight: 0 + - serializedVersion: 3 + time: 1000 + value: 1000 + inSlope: 1 + outSlope: 0 + tangentMode: 0 + weightedMode: 0 + inWeight: 0 + outWeight: 0 + m_PreInfinity: 2 + m_PostInfinity: 2 + m_RotationOrder: 4 + curveMax: 1 + average: 0 + - rid: 2243601383627161706 + type: {class: NucleusArray, ns: , asm: Assembly-CSharp} + data: + _nuclei: + - rid: 2243601383627161705 + name: Output diff --git a/Identity.asset.meta b/Identity.asset.meta new file mode 100644 index 0000000..b2382a6 --- /dev/null +++ b/Identity.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 5f4d2ea0d0115b3549f8e9aa5e669163 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/LinearAlgebra.meta b/LinearAlgebra.meta new file mode 100644 index 0000000..c54c1af --- /dev/null +++ b/LinearAlgebra.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d98555a675e8e5e879de17db950b55fe +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/LinearAlgebra/.editorconfig b/LinearAlgebra/.editorconfig new file mode 100644 index 0000000..1ec7f97 --- /dev/null +++ b/LinearAlgebra/.editorconfig @@ -0,0 +1,19 @@ +# EditorConfig is awesome: https://EditorConfig.org + +# top-most EditorConfig file +root = true + +[*] +indent_style = space +indent_size = 4 +end_of_line = crlf +charset = utf-8 +trim_trailing_whitespace = false +insert_final_newline = false +max_line_length = 80 + +[*.cs] +csharp_new_line_before_open_brace = none +# Suppress warnings everywhere +dotnet_diagnostic.IDE1006.severity = none +dotnet_diagnostic.IDE0130.severity = none \ No newline at end of file diff --git a/LinearAlgebra/.gitea/workflows/unit_tests.yaml b/LinearAlgebra/.gitea/workflows/unit_tests.yaml new file mode 100644 index 0000000..e98b26e --- /dev/null +++ b/LinearAlgebra/.gitea/workflows/unit_tests.yaml @@ -0,0 +1,37 @@ +name: Build and Run C# Unit Tests + +on: + push: + branches: + - '**' + pull_request: + branches: + - '**' + +jobs: + build-and-test: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Setup .NET + uses: actions/setup-dotnet@v1 + with: + dotnet-version: '8.0.x' # Specify the .NET SDK version + + - name: Check Current Directory + run: pwd # Logs the current working directory + + - name: List Files + run: ls -la # Lists all files in the current directory + + - name: Restore Dependencies + run: dotnet restore ./LinearAlgebra-csharp.sln # Restore NuGet packages + + - name: Build the Project + run: dotnet build ./LinearAlgebra-csharp.sln --configuration Release # Build the C# project + + - name: Run Unit Tests + run: dotnet test ./test/LinearAlgebra_Test.csproj --configuration Release # Execute unit tests diff --git a/LinearAlgebra/.gitignore b/LinearAlgebra/.gitignore new file mode 100644 index 0000000..b32a30c --- /dev/null +++ b/LinearAlgebra/.gitignore @@ -0,0 +1,5 @@ +DoxyGen/DoxyWarnLogfile.txt +.vscode/settings.json +**bin +**obj +**.meta diff --git a/LinearAlgebra/LinearAlgebra-csharp.sln b/LinearAlgebra/LinearAlgebra-csharp.sln new file mode 100644 index 0000000..4b13b2b --- /dev/null +++ b/LinearAlgebra/LinearAlgebra-csharp.sln @@ -0,0 +1,30 @@ +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.5.2.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra", "src\LinearAlgebra.csproj", "{ECB58727-0354-924D-AE7B-22F6B21097EB}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LinearAlgebra_Test", "test\LinearAlgebra_Test.csproj", "{715BB399-5FC4-2AC9-3757-177CA0C80774}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {ECB58727-0354-924D-AE7B-22F6B21097EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {ECB58727-0354-924D-AE7B-22F6B21097EB}.Debug|Any CPU.Build.0 = Debug|Any CPU + {ECB58727-0354-924D-AE7B-22F6B21097EB}.Release|Any CPU.ActiveCfg = Release|Any CPU + {ECB58727-0354-924D-AE7B-22F6B21097EB}.Release|Any CPU.Build.0 = Release|Any CPU + {715BB399-5FC4-2AC9-3757-177CA0C80774}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {715BB399-5FC4-2AC9-3757-177CA0C80774}.Debug|Any CPU.Build.0 = Debug|Any CPU + {715BB399-5FC4-2AC9-3757-177CA0C80774}.Release|Any CPU.ActiveCfg = Release|Any CPU + {715BB399-5FC4-2AC9-3757-177CA0C80774}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {E93B8294-87D4-4887-83B7-182A623D5833} + EndGlobalSection +EndGlobal diff --git a/LinearAlgebra/src/Angle.cs b/LinearAlgebra/src/Angle.cs new file mode 100644 index 0000000..7d2fd6b --- /dev/null +++ b/LinearAlgebra/src/Angle.cs @@ -0,0 +1,341 @@ +using System; + +namespace LinearAlgebra { + + public struct AngleFloat { + public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; + public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F; + + private AngleFloat(float degrees) { + this.value = degrees; + } + private readonly float value; + + public static AngleFloat Degrees(float degrees) { + // Reduce it to (-180..180] + if (float.IsFinite(degrees)) { + while (degrees < -180) + degrees += 360; + while (degrees >= 180) + degrees -= 360; + } + return new AngleFloat(degrees); + } + + public static AngleFloat Radians(float radians) { + // Reduce it to (-pi..pi] + if (float.IsFinite(radians)) { + while (radians <= -Math.PI) + radians += 2 * (float)Math.PI; + while (radians > Math.PI) + radians -= 2 * (float)Math.PI; + } + + return new AngleFloat(radians * Rad2Deg); + } + + public static AngleFloat Revolutions(float revolutions) { + // reduce it to (-0.5 .. 0.5] + if (float.IsFinite(revolutions)) { + // Get the integer part + int integerPart = (int)revolutions; + + // Get the decimal part + revolutions -= integerPart; + if (revolutions < -0.5) + revolutions += 1; + if (revolutions >= 0.5) + revolutions -= 1; + } + return new AngleFloat(revolutions * 360); + } + + public readonly float inDegrees => this.value; + + public readonly float inRadians => this.value * Deg2Rad; + + public readonly float inRevolutions => this.value / 360.0f; + + public override string ToString() { + return $"{this.inDegrees}\u00B0"; + } + + public static readonly AngleFloat zero = Degrees(0); + public static readonly AngleFloat deg90 = Degrees(90); + public static readonly AngleFloat deg180 = Degrees(180); + + /// + /// Get the sign of the angle + /// + /// The angle + /// -1 when the angle is negative, 1 when it is positive and 0 in all other cases + public static int Sign(AngleFloat a) { + if (a.value < 0) + return -1; + if (a.value > 0) + return 1; + return 0; + } + + /// + /// Returns the magnitude of the angle + /// + /// The angle + /// The positive magnitude of the angle + /// Negative values are negated to get a positive result + public static AngleFloat Abs(AngleFloat a) { + if (Sign(a) < 0) + return -a; + else + return a; + } + + /// + /// Tests the equality of two angles + /// + /// + /// + /// True when the angles are equal, false otherwise + /// The equality is determine within the limits of precision of a float + public static bool operator ==(AngleFloat a1, AngleFloat a2) { + return a1.value == a2.value; + } + + /// + /// Tests the inequality of two angles + /// + /// + /// + /// True when the angles are not equal, false otherwise + /// The equality is determine within the limits of precision of a float + public static bool operator !=(AngleFloat a1, AngleFloat a2) { + return a1.value != a2.value; + } + + public override readonly bool Equals(object obj) { + if (obj is AngleFloat other) { + return this == other; + } + return false; + } + + public override readonly int GetHashCode() { + return this.value.GetHashCode(); + } + + + /// + /// Tests if the first angle is greater than the second + /// + /// + /// + /// True when a1 is greater than a2, False otherwise + public static bool operator >(AngleFloat a1, AngleFloat a2) { + return a1.value > a2.value; + } + + /// + /// Tests if the first angle is greater than or equal to the second + /// + /// + /// + /// True when a1 is greater than or equal to a2, False otherwise + public static bool operator >=(AngleFloat a1, AngleFloat a2) { + return a1.value >= a2.value; + } + + /// + /// Tests if the first angle is less than the second + /// + /// + /// + /// True when a1 is less than a2, False otherwise + public static bool operator <(AngleFloat a1, AngleFloat a2) { + return a1.value < a2.value; + } + + /// + /// Tests if the first angle is less than or equal to the second + /// + /// + /// + /// True when a1 is less than or equal to a2, False otherwise + public static bool operator <=(AngleFloat a1, AngleFloat a2) { + return a1.value <= a2.value; + } + + /// + /// Negate the angle + /// + /// The angle + /// The negated angle + /// The negation of -180 is still -180 because the range is (-180..180] + public static AngleFloat operator -(AngleFloat a) { + AngleFloat r = new(-a.value); + return r; + } + + /// + /// Subtract two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the subtraction + public static AngleFloat operator -(AngleFloat a1, AngleFloat a2) { + AngleFloat r = new(a1.value - a2.value); + return r; + } + /// + /// Add two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the addition + public static AngleFloat operator +(AngleFloat a1, AngleFloat a2) { + AngleFloat r = new(a1.value + a2.value); + return r; + } + + /// + /// Multiplies the angle + /// + /// The angle to multiply + /// The factor by which the angle is multiplied + /// The multiplied angle + public static AngleFloat operator *(AngleFloat a, float factor) { + return Degrees(a.inDegrees * factor); + } + public static AngleFloat operator *(float factor, AngleFloat a) { + return Degrees(factor * a.inDegrees); + } + + /// + /// Clamp the angle between the given min and max values + /// + /// The angle to clamp + /// The minimum angle + /// The maximum angle + /// The clamped angle + /// Angles are normalized + public static float Clamp(AngleFloat angle, AngleFloat min, AngleFloat max) { + return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees); + } + + /// @brief Calculates the cosine of an angle + /// @param angle The given angle + /// @return The cosine of the angle + public static float Cos(AngleFloat angle) { + return MathF.Cos(angle.inRadians); + } + /// @brief Calculates the sine of an angle + /// @param angle The given angle + /// @return The sine of the angle + public static float Sin(AngleFloat angle) { + return MathF.Sin(angle.inRadians); + } + /// @brief Calculates the tangent of an angle + /// @param angle The given angle + /// @return The tangent of the angle + public static float Tan(AngleFloat angle) { + return MathF.Tan(angle.inRadians); + } + + /// @brief Calculates the arc cosine angle + /// @param f The value + /// @return The arc cosine for the given value + public static AngleFloat Acos(float f) { + return Radians(MathF.Acos(f)); + } + /// @brief Calculates the arc sine angle + /// @param f The value + /// @return The arc sine for the given value + public static AngleFloat Asin(float f) { + return Radians(MathF.Asin(f)); + } + /// @brief Calculates the arc tangent angle + /// @param f The value + /// @return The arc tangent for the given value + public static AngleFloat Atan(float f) { + return Radians(MathF.Atan(f)); + } + /// @brief Calculates the tangent for the given values + /// @param y The vertical value + /// @param x The horizontal value + /// @return The tanget for the given values + /// Uses the y and x signs to compute the quadrant + public static AngleFloat Atan2(float y, float x) { + return Radians(MathF.Atan2(y, x)); + } + + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// Maximum angle to rotate + /// The resulting angle + /// This function is compatible with radian and degrees angles + public static AngleFloat MoveTowards(AngleFloat fromAngle, AngleFloat toAngle, float maxDegrees) { + maxDegrees = Math.Max(0, maxDegrees); // filter out negative distances + AngleFloat d = toAngle - fromAngle; + float dDegrees = Abs(d).inDegrees; + d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees)); + if (Sign(d) < 0) + d = -d; + return fromAngle + d; + } + } + + + /// + /// %Angle utilities + /// + public static class Angles { + public const float pi = 3.1415927410125732421875F; + // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); + // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; + + + /// + /// Determine the angle difference, result is a normalized angle + /// + /// First first angle + /// The second angle + /// the angle between the two angles + /// Angle values should be degrees + public static float Difference(float a, float b) { + float r = Normalize(b - a); + return r; + } + + /// + /// Normalize an angle to the range -180 < angle <= 180 + /// + /// The angle to normalize + /// The normalized angle in interval (-180..180] + /// Angle values should be in degrees + public static float Normalize(float angle) { + if (float.IsInfinity(angle)) + return angle; + + while (angle <= -180) angle += 360; + while (angle > 180) angle -= 360; + return angle; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// Vectors a and b must be normalized + /// \deprecated Please use Vector2.ToFactor instead. + // [Obsolete("Please use Vector2.ToFactor instead.")] + // public static float ToFactor(Vector2Float v1, Vector2Float v2) { + // return (1 - Vector2Float.Dot(v1, v2)) / 2; + // } + + } + +} \ No newline at end of file diff --git a/LinearAlgebra/src/Decomposition.cs b/LinearAlgebra/src/Decomposition.cs new file mode 100644 index 0000000..ddaf434 --- /dev/null +++ b/LinearAlgebra/src/Decomposition.cs @@ -0,0 +1,287 @@ +using System; +namespace LinearAlgebra { + class QR { + // QR Decomposition of a matrix A + public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) { + int nRows = A.nRows; + int nCols = A.nCols; + + float[,] Q = new float[nRows, nCols]; + float[,] R = new float[nCols, nCols]; + + // Perform Gram-Schmidt orthogonalization + for (uint colIx = 0; colIx < nCols; colIx++) { + + // Step 1: v = column(ix) of A + float[] v = new float[nRows]; + for (int rowIx = 0; rowIx < nRows; rowIx++) + v[rowIx] = A.data[rowIx, colIx]; + + // Step 2: Subtract projections of v onto previous q's (orthogonalize) + for (uint colIx2 = 0; colIx2 < colIx; colIx2++) { + float dotProd = 0; + for (int i = 0; i < nRows; i++) + dotProd += Q[i, colIx2] * v[i]; + for (int i = 0; i < nRows; i++) + v[i] -= dotProd * Q[i, colIx2]; + } + + // Step 3: Normalize v to get column(ix) of Q + float norm = 0; + for (int rowIx = 0; rowIx < nRows; rowIx++) + norm += v[rowIx] * v[rowIx]; + norm = (float)Math.Sqrt(norm); + + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = v[rowIx] / norm; + + // Store the coefficients of R + for (int colIx2 = 0; colIx2 <= colIx; colIx2++) { + R[colIx2, colIx] = 0; + for (int k = 0; k < nRows; k++) + R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx]; + } + } + return (new Matrix2(Q), new Matrix2(R)); + } + + // Reduced QR Decomposition of a matrix A + public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) { + int nRows = A.nRows; + int nCols = A.nCols; + + float[,] Q = new float[nRows, nCols]; + float[,] R = new float[nCols, nCols]; + + // Perform Gram-Schmidt orthogonalization + for (int colIx = 0; colIx < nCols; colIx++) { + + // Step 1: v = column(colIx) of A + float[] columnIx = new float[nRows]; + bool isZeroColumn = true; + for (int rowIx = 0; rowIx < nRows; rowIx++) { + columnIx[rowIx] = A.data[rowIx, colIx]; + if (columnIx[rowIx] != 0) + isZeroColumn = false; + } + if (isZeroColumn) { + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = 0; + // Set corresponding R element to 0 + R[colIx, colIx] = 0; + + Console.WriteLine($"zero column {colIx}"); + + continue; + } + + // Step 2: Subtract projections of v onto previous q's (orthogonalize) + for (int colIx2 = 0; colIx2 < colIx; colIx2++) { + // Compute the dot product of v and column(colIx2) of Q + float dotProduct = 0; + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2]; + // Subtract the projection from v + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2]; + } + + // Step 3: Normalize v to get column(colIx) of Q + float norm = 0; + for (int rowIx = 0; rowIx < nRows; rowIx++) + norm += columnIx[rowIx] * columnIx[rowIx]; + if (norm == 0) + throw new Exception("invalid value"); + + norm = (float)Math.Sqrt(norm); + + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = columnIx[rowIx] / norm; + + // Here is where it deviates from the Full QR Decomposition ! + + // Step 4: Compute the row(colIx) of R + for (int colIx2 = colIx; colIx2 < nCols; colIx2++) { + float dotProduct = 0; + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2]; + R[colIx, colIx2] = dotProduct; + } + } + if (!float.IsFinite(R[0, 0])) + throw new Exception("invalid value"); + + return (new Matrix2(Q), new Matrix2(R)); + } + } + + class SVD { + // According to ChatGPT, Mathnet uses Golub-Reinsch SVD algorithm + // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization. + // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix. + // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD. + // + // 2. Diagonalization: Once the matrix is in bidiagonal form, + // the singular values are computed using an iterative process + // (typically involving QR factorization or Jacobi rotations) until convergence. + // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values. + // + // 3. Computing UU and VTVT: After obtaining the singular values, + // the left singular vectors UU and right singular vectors VTVT are computed + // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step. + + // Bidiagnolizations through Householder transformations + public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) { + int m = A.nRows; // Rows of A + int n = A.nCols; // Columns of A + float[,] U1 = new float[m, m]; // Left orthogonal matrix + float[,] V1 = new float[n, n]; // Right orthogonal matrix + float[,] B = A.Clone().data; // Copy A to B for transformation + + // Initialize U1 and V1 as identity matrices + for (int i = 0; i < m; i++) + U1[i, i] = 1; + for (int i = 0; i < n; i++) + V1[i, i] = 1; + + // Perform Householder reflections to create a bidiagonal matrix B + for (int j = 0; j < n; j++) { + // Step 1: Construct the Householder vector y + float[] y = new float[m - j]; + for (int i = j; i < m; i++) + y[i - j] = B[i, j]; + + // Step 2: Compute the norm and scalar alpha + float norm = 0; + for (int i = 0; i < y.Length; i++) + norm += y[i] * y[i]; + norm = (float)Math.Sqrt(norm); + + if (B[j, j] > 0) + norm = -norm; + + float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j]))); + float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); + + // Step 3: Apply the reflection to zero out below diagonal + for (int k = j; k < n; k++) { + float dot = 0; + for (int i = j; i < m; i++) + dot += y[i - j] * B[i, k]; + dot /= r; + + for (int i = j; i < m; i++) + B[i, k] -= 2 * dot * y[i - j]; + } + + // Step 4: Update U1 with the Householder reflection (U1 * Householder) + for (int i = j; i < m; i++) + U1[i, j] = y[i - j] / alpha; + + // Step 5: Update V1 (storing the Householder vector y) + // Correct indexing: we only need to store part of y in V1 from index j to n + for (int i = j; i < n; i++) + V1[j, i] = B[j, i]; + + // Repeat steps for further columns if necessary + } + return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1)); + } + + public static Matrix2 Bidiagonalize(Matrix2 A) { + int m = A.nRows; // Rows of A + int n = A.nCols; // Columns of A + float[,] B = A.Clone().data; // Copy A to B for transformation + + // Perform Householder reflections to create a bidiagonal matrix B + for (int j = 0; j < n; j++) { + // Step 1: Construct the Householder vector y + float[] y = new float[m - j]; + for (int i = j; i < m; i++) + y[i - j] = B[i, j]; + + // Step 2: Compute the norm and scalar alpha + float norm = 0; + for (int i = 0; i < y.Length; i++) + norm += y[i] * y[i]; + norm = (float)Math.Sqrt(norm); + + if (B[j, j] > 0) + norm = -norm; + + float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); + + // Step 3: Apply the reflection to zero out below diagonal + for (int k = j; k < n; k++) { + float dot = 0; + for (int i = j; i < m; i++) + dot += y[i - j] * B[i, k]; + dot /= r; + + for (int i = j; i < m; i++) + B[i, k] -= 2 * dot * y[i - j]; + } + + // Repeat steps for further columns if necessary + } + return new Matrix2(B); + } + + // QR Iteration for diagonalization of a bidiagonal matrix B + public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) { + int m = B.nRows; + int n = B.nCols; + + Matrix2 U = new(m, m); // Left singular vectors (U) + Matrix2 Vt = new(n, n); // Right singular vectors (V^T) + float[] singularValues = new float[Math.Min(m, n)]; // Singular values + + // Initialize U and Vt as identity matrices + for (int i = 0; i < m; i++) + U.data[i, i] = 1; + for (int i = 0; i < n; i++) + Vt.data[i, i] = 1; + + // Perform QR iterations + float tolerance = 1e-7f; //1e-12f; for double + bool converged = false; + while (!converged) { + // Perform QR decomposition on the matrix B + (Matrix2 Q, Matrix2 R) = QR.Decomposition(B); + + // Update B to be the product Q * R //R * Q + B = R * Q; + + // Accumulate the transformations in U and Vt + U *= Q; + Vt *= R; + + // Check convergence by looking at the off-diagonal elements of B + converged = true; + for (int i = 0; i < m - 1; i++) { + for (int j = i + 1; j < n; j++) { + if (Math.Abs(B.data[i, j]) > tolerance) { + converged = false; + break; + } + } + } + } + + // Extract singular values (diagonal elements of B) + for (int i = 0; i < Math.Min(m, n); i++) + singularValues[i] = B.data[i, i]; + + return (new Matrix1(singularValues), U, Vt); + } + + public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) { + if (A.nRows != A.nCols) + throw new ArgumentException("SVD: matrix A has to be square."); + + Matrix2 B = Bidiagonalize(A); + (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B); + return (U, S, Vt); + } + } +} \ No newline at end of file diff --git a/LinearAlgebra/src/Direction.cs b/LinearAlgebra/src/Direction.cs new file mode 100644 index 0000000..fd25b98 --- /dev/null +++ b/LinearAlgebra/src/Direction.cs @@ -0,0 +1,261 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra +{ + + /// + /// A direction in 3D space + /// + /// A direction is represented using two angles: + /// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive) + /// degrees which is a rotation in the horizontal plane + /// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees + /// which is the rotation in the up/down direction applied after the horizontal + /// rotation has been applied. + /// The angles are automatically normalized to stay within the abovenmentioned + /// ranges. + public struct Direction + { + /// @brief horizontal angle, range = (-180..180] degrees + public AngleFloat horizontal; + /// @brief vertical angle, range in degrees = (-90..90] degrees + public AngleFloat vertical; + + /// + /// Create a new direction + /// + /// The horizontal angle + /// The vertical angle + /// The direction will be normalized automatically + /// to ensure the angles are within the allowed ranges + public Direction(AngleFloat horizontal, AngleFloat vertical) + { + this.horizontal = horizontal; + this.vertical = vertical; + this.Normalize(); + } + + /// + /// Create a direction using angle values in degrees + /// + /// The horizontal angle in degrees + /// The vertical angle in degrees + /// The direction + /// The direction will be normalized automatically + /// to ensure the angles are within the allowed ranges + public static Direction Degrees(float horizontal, float vertical) + { + Direction d = new() + { + horizontal = AngleFloat.Degrees(horizontal), + vertical = AngleFloat.Degrees(vertical) + }; + d.Normalize(); + return d; + } + /// + /// Create a direction using angle values in radians + /// + /// The horizontal angle in radians + /// The vertical angle in radians + /// The direction + public static Direction Radians(float horizontal, float vertical) + { + Direction d = new() + { + horizontal = AngleFloat.Radians(horizontal), + vertical = AngleFloat.Radians(vertical) + }; + d.Normalize(); + return d; + } + + public override readonly string ToString() { + return $"Direction(h: {this.horizontal}, v: {this.vertical})"; + } + + /// + /// A forward direction with zero for both angles + /// + public readonly static Direction forward = Degrees(0, 0); + /// + /// A backward direction with horizontal angle -180 and zero vertical + /// angle + /// + public readonly static Direction backward = Degrees(-180, 0); + /// + /// A upward direction with zero horizontal angle and vertical angle 90 + /// + public readonly static Direction up = Degrees(0, 90); + /// + /// A downward direction with zero horizontal angle and vertical angle + /// -90 + /// + public readonly static Direction down = Degrees(0, -90); + /// + /// A left-pointing direction with horizontal angle -90 and zero + /// vertical angle + /// + public readonly static Direction left = Degrees(-90, 0); + /// + /// A right-pointing direction with horizontal angle 90 and zero + /// vertical angle + /// + public readonly static Direction right = Degrees(90, 0); + + private void Normalize() + { + if (this.vertical > AngleFloat.deg90 || this.vertical < -AngleFloat.deg90) + { + this.horizontal += AngleFloat.deg180; + this.vertical = AngleFloat.Degrees(180 - this.vertical.inDegrees); + } + } + +#if UNITY_5_3_OR_NEWER + /// + /// Convert the direction into a carthesian vector + /// + /// The carthesian vector corresponding to this direction. + public readonly UnityEngine.Vector3 ToVector3() { + // Convert degrees to radians + float radH = this.horizontal.inRadians; + float radV = this.vertical.inRadians; + + // Calculate Vector + float cosV = MathF.Cos(radV); + float sinV = MathF.Sin(radV); + + float x = cosV * MathF.Sin(radH); + float y = sinV; + float z = cosV * MathF.Cos(radH); + + return new UnityEngine.Vector3(x, y, z); + } + + /// + /// Convert a carthesian vector into a direction + /// + /// The carthesian vector + /// The direction + /// Information about the length of the carthesian vector is not + /// included in this transformation + public static Direction FromVector3(UnityEngine.Vector3 v) { + AngleFloat horizontal = AngleFloat.Atan2(v.x, v.z); + AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.y); + Direction d = new(horizontal, vertical); + return d; + } +#else + /// + /// Convert the direction into a carthesian vector + /// + /// The carthesian vector corresponding to this direction. + public readonly Vector3Float ToVector3() { + // Quaternion q = Quaternion.Euler(90 - this.vertical.inDegrees, this.horizontal.inDegrees, 0); + // Vector3Float v = q * Vector3Float.forward; + // return v; + + float radH = this.horizontal.inRadians; + float radV = this.vertical.inRadians; + + float cosV = MathF.Cos(radV); + float sinV = MathF.Sin(radV); + + float horizontal = cosV * MathF.Sin(radH); + float vertical = sinV; + float depth = cosV * MathF.Cos(radH); + + return new Vector3Float(horizontal, vertical, depth); + } + + /// + /// Convert a carthesian vector into a direction + /// + /// The carthesian vector + /// The direction + /// Information about the length of the carthesian vector is not + /// included in this transformation + public static Direction FromVector3(Vector3Float v) + { + AngleFloat horizontal = AngleFloat.Atan2(v.horizontal, v.depth); + AngleFloat vertical = AngleFloat.deg90 - AngleFloat.Acos(v.vertical); + Direction d = new(horizontal, vertical); + return d; + } +#endif + + public static Direction operator -(Direction d) { + AngleFloat horizontal = d.horizontal + AngleFloat.deg180; + AngleFloat vertical = -d.vertical; + return new Direction(horizontal, vertical); + } + + /// + /// Tests the equality of two directions + /// + /// + /// + /// True when the direction angles are equal, false otherwise. + public static bool operator ==(Direction d1, Direction d2) + { + bool horizontalEq = d1.horizontal == d2.horizontal; + bool verticalEq = d1.vertical == d2.vertical; + return horizontalEq && verticalEq; + } + + /// + /// Tests the inequality of two directions + /// + /// + /// + /// True when the direction angles are not equal, false otherwise. + public static bool operator !=(Direction d1, Direction d2) + { + bool horizontalNEq = d1.horizontal != d2.horizontal; + bool verticalNEq = d1.vertical != d2.vertical; + return horizontalNEq || verticalNEq; + } + + public override readonly bool Equals(object obj) + { + if (obj is not Direction d) + return false; + + bool horizontalEq = this.horizontal == d.horizontal; + bool verticalEq = this.vertical == d.vertical; + return horizontalEq && verticalEq; + } + + + public override readonly int GetHashCode() + { + return HashCode.Combine(horizontal, vertical); + } + + public static AngleFloat UnsignedAngle(Direction d1, Direction d2) { + // Convert angles from degrees to radians + float horizontal1Rad = d1.horizontal.inRadians; + float vertical1Rad = d1.vertical.inRadians; + + float horizontal2Rad = d2.horizontal.inRadians; + float vertical2Rad = d2.vertical.inRadians; + + // Calculate the cosine of the angle using the spherical law of cosines + float cosTheta = MathF.Sin(vertical1Rad) * MathF.Sin(vertical2Rad) + + MathF.Cos(vertical1Rad) * MathF.Cos(vertical2Rad) * + MathF.Cos(horizontal1Rad - horizontal2Rad); + + // Clip cosTheta to the valid range for acos + cosTheta = Float.Clamp(cosTheta, -1.0f, 1.0f); + + // Calculate the angle + AngleFloat angle = AngleFloat.Acos(cosTheta); + return angle; + } + } + +} \ No newline at end of file diff --git a/LinearAlgebra/src/Float.cs b/LinearAlgebra/src/Float.cs new file mode 100644 index 0000000..2217b84 --- /dev/null +++ b/LinearAlgebra/src/Float.cs @@ -0,0 +1,41 @@ +namespace LinearAlgebra { + + /// + /// Float number utilities + /// + public class Float { + /// + /// The precision of float numbers + /// + public const float epsilon = 1E-05f; + /// + /// The square of the float number precision + /// + public const float sqrEpsilon = 1e-10f; + + /// + /// Clamp the value between the given minimum and maximum values + /// + /// The value to clamp + /// The minimum value + /// The maximum value + /// The clamped value + public static float Clamp(float f, float min, float max) { + if (f < min) + return min; + if (f > max) + return max; + return f; + } + + /// + /// Clamp the value between to the interval [0..1] + /// + /// The value to clamp + /// The clamped value + public static float Clamp01(float f) { + return Clamp(f, 0, 1); + } + } + +} \ No newline at end of file diff --git a/LinearAlgebra/src/LinearAlgebra.csproj b/LinearAlgebra/src/LinearAlgebra.csproj new file mode 100644 index 0000000..d2d5a85 --- /dev/null +++ b/LinearAlgebra/src/LinearAlgebra.csproj @@ -0,0 +1,14 @@ + + + + false + false + net8.0 + + + + + + + + diff --git a/LinearAlgebra/src/Matrix.cs b/LinearAlgebra/src/Matrix.cs new file mode 100644 index 0000000..37c6c24 --- /dev/null +++ b/LinearAlgebra/src/Matrix.cs @@ -0,0 +1,689 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +using Vector2Float = UnityEngine.Vector2; +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra { + + public readonly struct Slice + { + public int start { get; } + public int stop { get; } + public Slice(int start, int stop) + { + this.start = start; + this.stop = stop; + } + } + + public class Matrix2 { + public float[,] data { get; } + + public int nRows => data.GetLength(0); + public int nCols => data.GetLength(1); + + public Matrix2(int nRows, int nCols) + { + this.data = new float[nRows, nCols]; + } + public Matrix2(float[,] data) { + this.data = data; + } + + public Matrix2 Clone() { + float[,] data = new float[this.nRows, nCols]; + for (int rowIx = 0; rowIx < this.nRows; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) + data[rowIx, colIx] = this.data[rowIx, colIx]; + } + return new Matrix2(data); + } + + public static Matrix2 Zero(int nRows, int nCols) + { + return new Matrix2(nRows, nCols); + } + + public static Matrix2 FromVector3(Vector3Float v) { + float[,] result = new float[3, 1]; + result[0, 0] = v.horizontal; + result[1, 0] = v.vertical; + result[2, 0] = v.depth; + return new Matrix2(result); + } + + public static Matrix2 Identity(int size) + { + return Diagonal(1, size); + } + public static Matrix2 Identity(int nRows, int nCols) + { + Matrix2 m = Zero(nRows, nCols); + m.FillDiagonal(1); + return m; + } + + public static Matrix2 Diagonal(Matrix1 v) { + float[,] resultData = new float[v.size, v.size]; + for (int ix = 0; ix < v.size; ix++) + resultData[ix, ix] = v.data[ix]; + return new Matrix2(resultData); + } + public static Matrix2 Diagonal(float f, int size) + { + float[,] resultData = new float[size, size]; + for (int ix = 0; ix < size; ix++) + resultData[ix, ix] = f; + return new Matrix2(resultData); + } + public void FillDiagonal(Matrix1 v) + { + int n = (int)Math.Min(Math.Min(this.nRows, this.nCols), v.size); + for (int ix = 0; ix < n; ix++) + this.data[ix, ix] = v.data[ix]; + } + public void FillDiagonal(float f) + { + int n = Math.Min(this.nRows, this.nCols); + for (int ix = 0; ix < n; ix++) + this.data[ix, ix] = f; + } + + public static Matrix2 SkewMatrix(Vector3Float v) { + float[,] result = new float[3, 3] { + {0, -v.depth, v.vertical}, + {v.depth, 0, -v.horizontal}, + {-v.vertical, v.horizontal, 0} + }; + return new Matrix2(result); + } + + public Matrix1 GetRow(int rowIx) { + float[] row = new float[this.nCols]; + for (int colIx = 0; colIx < this.nCols; colIx++) { + row[colIx] = this.data[rowIx, colIx]; + } + return new Matrix1(row); + } + +#if UNITY_5_3_OR_NEWER + public UnityEngine.Vector3 GetRow3(int rowIx) { + int cols = this.nCols; + UnityEngine.Vector3 row = new() { + x = this.data[rowIx, 0], + y = this.data[rowIx, 1], + z = this.data[rowIx, 2] + }; + return row; + } +#endif + public void SetRow(int rowIx, Matrix1 v) { + for (uint ix = 0; ix < v.size; ix++) + this.data[rowIx, ix] = v.data[ix]; + } + public void SetRow3(int rowIx, Vector3Float v) { + this.data[rowIx, 0] = v.horizontal; + this.data[rowIx, 1] = v.vertical; + this.data[rowIx, 2] = v.depth; + } + + public void SwapRows(int row1, int row2) { + for (uint ix = 0; ix < this.nCols; ix++) { + float temp = this.data[row1, ix]; + this.data[row1, ix] = this.data[row2, ix]; + this.data[row2, ix] = temp; + } + } + + public Matrix1 GetColumn(int colIx) + { + float[] column = new float[this.nRows]; + for (int i = 0; i < this.nRows; i++) { + column[i] = this.data[i, colIx]; + } + return new Matrix1(column); + } + public void SetColumn(int colIx, Matrix1 v) { + for (uint ix = 0; ix < v.size; ix++) + this.data[ix, colIx] = v.data[ix]; + } + public void SetColumn(int colIx, Vector3Float v) { + this.data[0, colIx] = v.horizontal; + this.data[1, colIx] = v.vertical; + this.data[2, colIx] = v.depth; + } + + public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f) { + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) { + float d = MathF.Abs(A.data[i, j] - B.data[i, j]); + if (d > atol) + return false; + } + } + return true; + } + + public Matrix2 Transpose() { + float[,] resultData = new float[this.nCols, this.nRows]; + for (uint rowIx = 0; rowIx < this.nRows; rowIx++) { + for (uint colIx = 0; colIx < this.nCols; colIx++) + resultData[colIx, rowIx] = this.data[rowIx, colIx]; + } + return new Matrix2(resultData); + // double checked code + } + public Matrix2 transposed { + get => Transpose(); + } + + public static Matrix2 operator -(Matrix2 m) { + float[,] result = new float[m.nRows, m.nCols]; + + for (int i = 0; i < m.nRows; i++) { + for (int j = 0; j < m.nCols; j++) + result[i, j] = -m.data[i, j]; + } + return new Matrix2(result); + } + + public static Matrix2 operator -(Matrix2 A, Matrix2 B) { + if (A.nRows != B.nRows || A.nCols != B.nCols) + throw new System.ArgumentException("Size of A must match size of B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] - B.data[i, j]; + } + return new Matrix2(result); + } + + public static Matrix2 operator +(Matrix2 A, Matrix2 B) { + if (A.nRows != B.nRows || A.nCols != B.nCols) + throw new System.ArgumentException("Size of A must match size of B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] + B.data[i, j]; + } + return new Matrix2(result); + } + + public static Matrix2 operator *(Matrix2 A, Matrix2 B) { + if (A.nCols != B.nRows) + throw new System.ArgumentException("Number of columns in A must match number of rows in B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < B.nCols; j++) { + float sum = 0.0f; + for (int k = 0; k < A.nCols; k++) + sum += A.data[i, k] * B.data[k, j]; + + result[i, j] = sum; + } + } + + return new Matrix2(result); + // double checked code + } + + public static Matrix1 operator *(Matrix2 A, Matrix1 v) { + float[] result = new float[A.nRows]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) { + result[i] += A.data[i, j] * v.data[j]; + } + } + + return new Matrix1(result); + } + + public static Vector3Float operator *(Matrix2 A, Vector3Float v) { + return new Vector3Float( + A.data[0, 0] * v.horizontal + A.data[0, 1] * v.vertical + A.data[0, 2] * v.depth, + A.data[1, 0] * v.horizontal + A.data[1, 1] * v.vertical + A.data[1, 2] * v.depth, + A.data[2, 0] * v.horizontal + A.data[2, 1] * v.vertical + A.data[2, 2] * v.depth + ); + } + + public static Matrix2 operator *(Matrix2 A, float s) { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] * s; + } + + return new Matrix2(result); + } + public static Matrix2 operator *(float s, Matrix2 A) { + return A * s; + } + + public static Matrix2 operator /(Matrix2 A, float s) { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] / s; + } + + return new Matrix2(result); + } + public static Matrix2 operator /(float s, Matrix2 A) { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = s / A.data[i, j]; + } + + return new Matrix2(result); + } + + public Matrix2 GetRows(Slice slice) { + return GetRows(slice.start, slice.stop); + } + public Matrix2 GetRows(int from, int to) { + if (from < 0 || to >= this.nRows) + throw new System.ArgumentException("Slice index out of range."); + + float[,] result = new float[to - from, this.nCols]; + int resultRowIx = 0; + for (int rowIx = from; rowIx < to; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) + result[resultRowIx, colIx] = this.data[rowIx, colIx]; + + resultRowIx++; + } + + return new Matrix2(result); + } + + public Matrix2 Slice(Slice slice) + { + return Slice(slice.start, slice.stop); + } + public Matrix2 Slice(int from, int to) + { + if (from < 0 || to >= this.nRows) + throw new System.ArgumentException("Slice index out of range."); + + float[,] result = new float[to - from, this.nCols]; + int resultRowIx = 0; + for (int rowIx = from; rowIx < to; rowIx++) + { + for (int colIx = 0; colIx < this.nCols; colIx++) + { + result[resultRowIx, colIx] = this.data[rowIx, colIx]; + } + resultRowIx++; + } + + return new Matrix2(result); + } + public Matrix2 Slice(Slice rowRange, Slice colRange) { + return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop)); + } + public Matrix2 Slice((int start, int stop) rowRange, (int start, int stop) colRange) + { + float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start]; + + int resultRowIx = 0; + int resultColIx = 0; + for (int i = rowRange.start; i < rowRange.stop; i++) + { + for (int j = colRange.start; j < colRange.stop; j++) + result[resultRowIx, resultColIx] = this.data[i, j]; + } + return new Matrix2(result); + } + + public void UpdateSlice(Slice slice, Matrix2 m) { + UpdateSlice((slice.start, slice.stop), m); + } + public void UpdateSlice((int start, int stop) slice, Matrix2 m) { + // if (slice.start == slice.stop) + // Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!"); + int mRowIx = 0; + for (int rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) + this.data[rowIx, colIx] = m.data[mRowIx, colIx]; + } + } + + public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) + { + UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m); + } + public void UpdateSlice((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m) + { + for (int i = rowRange.start; i < rowRange.stop; i++) + { + for (int j = colRange.start; j < colRange.stop; j++) + this.data[i, j] = m.data[i - rowRange.start, j - colRange.start]; + } + } + + public Matrix2 Inverse() { + Matrix2 A = this; + // unchecked + int n = A.nRows; + + // Create an identity matrix of the same size as the original matrix + float[,] augmentedMatrix = new float[n, 2 * n]; + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + augmentedMatrix[i, j] = A.data[i, j]; + augmentedMatrix[i, j + n] = (i == j) ? 1 : 0; // Identity matrix + } + } + + // Perform Gaussian elimination + for (int i = 0; i < n; i++) { + // Find the pivot row + float pivot = augmentedMatrix[i, i]; + if (Math.Abs(pivot) < 1e-10) // Check for singular matrix + throw new InvalidOperationException("Matrix is singular and cannot be inverted."); + + // Normalize the pivot row + for (int j = 0; j < 2 * n; j++) + augmentedMatrix[i, j] /= pivot; + + // Eliminate the column below the pivot + for (int j = i + 1; j < n; j++) { + float factor = augmentedMatrix[j, i]; + for (int k = 0; k < 2 * n; k++) + augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k]; + } + } + + // Back substitution + for (int i = n - 1; i >= 0; i--) + { + // Eliminate the column above the pivot + for (int j = i - 1; j >= 0; j--) + { + float factor = augmentedMatrix[j, i]; + for (int k = 0; k < 2 * n; k++) + augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k]; + } + } + + // Extract the inverse matrix from the augmented matrix + float[,] inverse = new float[n, n]; + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) + inverse[i, j] = augmentedMatrix[i, j + n]; + } + + return new Matrix2(inverse); + } + + public float Determinant() + { + int n = this.nRows; + if (n != this.nCols) + throw new System.ArgumentException("Matrix must be square."); + + if (n == 1) + return this.data[0, 0]; // Base case for 1x1 matrix + + if (n == 2) // Base case for 2x2 matrix + return this.data[0, 0] * this.data[1, 1] - this.data[0, 1] * this.data[1, 0]; + + float det = 0; + for (int col = 0; col < n; col++) + det += (col % 2 == 0 ? 1 : -1) * this.data[0, col] * this.Minor(0, col).Determinant(); + + return det; + } + + // Helper function to compute the minor of a matrix + private Matrix2 Minor(int rowToRemove, int colToRemove) + { + int n = this.nRows; + float[,] minor = new float[n - 1, n - 1]; + + int r = 0, c = 0; + for (int i = 0; i < n; i++) { + if (i == rowToRemove) continue; + + c = 0; + for (int j = 0; j < n; j++) { + if (j == colToRemove) continue; + + minor[r, c] = this.data[i, j]; + c++; + } + r++; + } + + return new Matrix2(minor); + } + + public static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) { + float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols]; + + int resultRowIx = 0; + for (int i = 0; i < A.nRows; i++) { + if (i >= rowRange.start && i < rowRange.stop) + continue; + + for (int j = 0; j < A.nCols; j++) + result[resultRowIx, j] = A.data[i, j]; + + resultRowIx++; + } + return new Matrix2(result); + } + + internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) { + float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)]; + + for (int i = 0; i < A.nRows; i++) { + int resultColIx = 0; + for (int j = 0; j < A.nCols; j++) { + if (j >= colRange.start && j < colRange.stop) + continue; + + result[i, resultColIx++] = A.data[i, j]; + } + } + return new Matrix2(result); + } + } + + public class Matrix1 + { + public float[] data { get; } + + public int size => data.GetLength(0); + + public Matrix1(int size) + { + this.data = new float[size]; + } + + public Matrix1(float[] data) { + this.data = data; + } + + public static Matrix1 Zero(int size) + { + return new Matrix1(size); + } + + public static Matrix1 FromVector2(Vector2Float v) { + float[] result = new float[2]; + result[0] = v.horizontal; + result[1] = v.vertical; + return new Matrix1(result); + } + + public static Matrix1 FromVector3(Vector3Float v) { + float[] result = new float[3]; + result[0] = v.horizontal; + result[1] = v.vertical; + result[2] = v.depth; + return new Matrix1(result); + } + +#if UNITY_5_3_OR_NEWER + public static Matrix1 FromQuaternion(Quaternion q) { + float[] result = new float[4]; + result[0] = q.x; + result[1] = q.y; + result[2] = q.z; + result[3] = q.w; + return new Matrix1(result); + } +#endif + + public Vector2Float vector2 { + get { + if (this.size != 2) + throw new System.ArgumentException("Matrix1 must be of size 2"); + return new Vector2Float(this.data[0], this.data[1]); + } + } + public Vector3Float vector3 { + get { + if (this.size != 3) + throw new System.ArgumentException("Matrix1 must be of size 3"); + return new Vector3Float(this.data[0], this.data[1], this.data[2]); + } + } + +#if UNITY_5_3_OR_NEWER + public Quaternion quaternion { + get { + if (this.size != 4) + throw new System.ArgumentException("Matrix1 must be of size 4"); + return new Quaternion(this.data[0], this.data[1], this.data[2], this.data[3]); + } + } +#endif + + public Matrix1 Clone() { + float[] data = new float[this.size]; + for (int rowIx = 0; rowIx < this.size; rowIx++) + data[rowIx] = this.data[rowIx]; + return new Matrix1(data); + } + + + public float magnitude { + get { + float sum = 0; + foreach (var elm in data) + sum += elm; + return sum / data.Length; + } + } + public static Matrix1 operator +(Matrix1 A, Matrix1 B) { + if (A.size != B.size) + throw new System.ArgumentException("Size of A must match size of B."); + + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) { + result[i] = A.data[i] + B.data[i]; + } + return new Matrix1(result); + } + + public Matrix2 Transpose() { + float[,] r = new float[1, this.size]; + for (uint colIx = 0; colIx < this.size; colIx++) + r[1, colIx] = this.data[colIx]; + + return new Matrix2(r); + } + + public static float Dot(Matrix1 a, Matrix1 b) { + if (a.size != b.size) + throw new System.ArgumentException("Vectors must be of the same length."); + + float result = 0.0f; + for (int i = 0; i < a.size; i++) { + result += a.data[i] * b.data[i]; + } + return result; + } + + public static Matrix1 operator -(Matrix1 A, Matrix1 B) { + if (A.size != B.size) + throw new System.ArgumentException("Size of A must match size of B."); + + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) { + result[i] = A.data[i] - B.data[i]; + } + return new Matrix1(result); + } + + public static Matrix1 operator *(Matrix1 A, float f) + { + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + result[i] += A.data[i] * f; + + return new Matrix1(result); + } + public static Matrix1 operator *(float f, Matrix1 A) { + return A * f; + } + + public static Matrix1 operator /(Matrix1 A, float f) { + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + result[i] = A.data[i] / f; + + return new Matrix1(result); + } + public static Matrix1 operator /(float f, Matrix1 A) { + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + result[i] = f / A.data[i]; + + return new Matrix1(result); + } + + public Matrix1 Slice(Slice range) + { + return Slice(range.start, range.stop); + } + public Matrix1 Slice(int from, int to) + { + if (from < 0 || to >= this.size) + throw new System.ArgumentException("Slice index out of range."); + + float[] result = new float[to - from]; + int resultIx = 0; + for (int ix = from; ix < to; ix++) + result[resultIx++] = this.data[ix]; + + return new Matrix1(result); + } + public void UpdateSlice(Slice slice, Matrix1 v) { + int vIx = 0; + for (int ix = slice.start; ix < slice.stop; ix++, vIx++) + this.data[ix] = v.data[vIx]; + } + } + +} \ No newline at end of file diff --git a/LinearAlgebra/src/Quat32.cs b/LinearAlgebra/src/Quat32.cs new file mode 100644 index 0000000..19ee9bc --- /dev/null +++ b/LinearAlgebra/src/Quat32.cs @@ -0,0 +1,87 @@ +using System; + +namespace LinearAlgebra { + public class Quat32 { + public float x; + public float y; + public float z; + public float w; + + public Quat32() { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } + + public Quat32(float x, float y, float z, float w) { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + public static Quat32 FromSwingTwist(SwingTwist s) { + Quat32 q32 = Quat32.Euler(-s.swing.vertical.inDegrees, s.swing.horizontal.inDegrees, s.twist.inDegrees); + return q32; + } + + public static Quat32 Euler(float yaw, float pitch, float roll) { + float rollOver2 = roll * AngleFloat.Deg2Rad * 0.5f; + float sinRollOver2 = (float)Math.Sin((float)rollOver2); + float cosRollOver2 = (float)Math.Cos((float)rollOver2); + float pitchOver2 = pitch * 0.5f; + float sinPitchOver2 = (float)Math.Sin((float)pitchOver2); + float cosPitchOver2 = (float)Math.Cos((float)pitchOver2); + float yawOver2 = yaw * 0.5f; + float sinYawOver2 = (float)Math.Sin((float)yawOver2); + float cosYawOver2 = (float)Math.Cos((float)yawOver2); + Quat32 result = new Quat32() { + w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + + sinYawOver2 * sinPitchOver2 * sinRollOver2, + x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + + cosYawOver2 * sinPitchOver2 * sinRollOver2, + y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - + sinYawOver2 * cosPitchOver2 * sinRollOver2, + z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - + sinYawOver2 * sinPitchOver2 * cosRollOver2 + }; + return result; + } + + public void ToAngles(out float right, out float up, out float forward) { + float test = this.x * this.y + this.z * this.w; + if (test > 0.499f) { // singularity at north pole + right = 0; + up = 2 * (float)Math.Atan2(this.x, this.w) * AngleFloat.Rad2Deg; + forward = 90; + return; + //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90); + } + else if (test < -0.499f) { // singularity at south pole + right = 0; + up = -2 * (float)Math.Atan2(this.x, this.w) * AngleFloat.Rad2Deg; + forward = -90; + return; + //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90); + } + else { + float sqx = this.x * this.x; + float sqy = this.y * this.y; + float sqz = this.z * this.z; + + right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * AngleFloat.Rad2Deg; + up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * AngleFloat.Rad2Deg; + forward = (float)Math.Asin(2 * test) * AngleFloat.Rad2Deg; + return; + // return Vector3( + // atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * + // Rad2Deg, + // atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * + // Rad2Deg, + // asinf(2 * test) * Angle.Rad2Deg); + } + } + + } +} \ No newline at end of file diff --git a/LinearAlgebra/src/Quaternion.cs b/LinearAlgebra/src/Quaternion.cs new file mode 100644 index 0000000..7936843 --- /dev/null +++ b/LinearAlgebra/src/Quaternion.cs @@ -0,0 +1,582 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra { + +#if UNITY_5_3_OR_NEWER + public class QuaternionExtensions { + public static Quaternion Reflect(Quaternion q) { + return new(-q.x, -q.y, -q.z, q.w); + } + + public static Matrix2 ToRotationMatrix(Quaternion q) { + float w = q.x, x = q.y, y = q.z, z = q.w; + + float[,] result = new float[,] + { + { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) }, + { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) }, + { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) } + }; + return new Matrix2(result); + } + + public static Quaternion FromRotationMatrix(Matrix2 m) { + float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2]; + float w, x, y, z; + + if (trace > 0) { + float s = 0.5f / (float)Math.Sqrt(trace + 1.0f); + w = 0.25f / s; + x = (m.data[2, 1] - m.data[1, 2]) * s; + y = (m.data[0, 2] - m.data[2, 0]) * s; + z = (m.data[1, 0] - m.data[0, 1]) * s; + } + else { + if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]); + w = (m.data[2, 1] - m.data[1, 2]) / s; + x = 0.25f * s; + y = (m.data[0, 1] + m.data[1, 0]) / s; + z = (m.data[0, 2] + m.data[2, 0]) / s; + } + else if (m.data[1, 1] > m.data[2, 2]) { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]); + w = (m.data[0, 2] - m.data[2, 0]) / s; + x = (m.data[0, 1] + m.data[1, 0]) / s; + y = 0.25f * s; + z = (m.data[1, 2] + m.data[2, 1]) / s; + } + else { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]); + w = (m.data[1, 0] - m.data[0, 1]) / s; + x = (m.data[0, 2] + m.data[2, 0]) / s; + y = (m.data[1, 2] + m.data[2, 1]) / s; + z = 0.25f * s; + } + } + + return new Quaternion(x, y, z, w); + } + } +#else + public struct Quaternion { + public float x; + public float y; + public float z; + public float w; + + /// + /// create a new quaternion with the given values + /// + /// x component + /// y component + /// z component + /// w component + public Quaternion(float x, float y, float z, float w) { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + /// + /// An identity quaternion + /// + public static readonly Quaternion identity = new(0, 0, 0, 1); + + private readonly Vector3Float xyz => new(x, y, z); + + private readonly float magnitude => MathF.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w); + + private readonly float sqrMagnitude => x * x + y * y + z * z + w * w; + + /// + /// Convert to unit quaternion + /// + /// This will preserve the orientation, + /// but ensures that it is a unit quaternion. + public readonly Quaternion normalized { + get { + float length = this.magnitude; + Quaternion q = new(this.x / length, this.y / length, this.z / length, this.w / length); + return q; + } + } + + /// + /// Convert to unity quaternion + /// + /// The quaternion to convert + /// A unit quaternion + /// This will preserve the orientation, + /// but ensures that it is a unit quaternion. + public static Quaternion Normalize(Quaternion q) { + return q.normalized; + } + + /// + /// Convert to euler angles + /// + /// The quaternion to convert + /// A vector containing euler angles + /// The euler angles performed in the order: Z, X, Y + public static Vector3Float ToAngles(Quaternion q) { + // Extract Euler angles in Unity order (X = pitch, Y = yaw, Z = roll), returned in degrees as (x,pitch),(y,yaw),(z,roll) + // Handle singularities/gimbal lock + float test = 2f * (q.w * q.x - q.y * q.z); + // clamp + if (test >= 1f) test = 1f; + if (test <= -1f) test = -1f; + + float pitch = MathF.Asin(test); // X + + float roll = MathF.Atan2(2f * (q.w * q.z + q.x * q.y), + 1f - 2f * (q.x * q.x + q.z * q.z)); // Z + + float yaw = MathF.Atan2(2f * (q.w * q.y + q.x * q.z), + 1f - 2f * (q.y * q.y + q.x * q.x)); // Y + + const float rad2deg = 180f / MathF.PI; + return new Vector3Float(pitch * rad2deg, yaw * rad2deg, roll * rad2deg); + // float test = q.x * q.y + q.z * q.w; + // if (test > 0.499f) // singularity at north pole + // return new Vector3Float(0, 2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, 90); + + // else if (test < -0.499f) // singularity at south pole + // return new Vector3Float(0, -2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, -90); + + // else { + // float sqx = q.x * q.x; + // float sqy = q.y * q.y; + // float sqz = q.z * q.z; + + // return new Vector3Float( + // MathF.Atan2(2 * q.x * q.w - 2 * q.y * q.z, 1 - 2 * sqx - 2 * sqz) * + // AngleFloat.Rad2Deg, + // MathF.Atan2(2 * q.y * q.w - 2 * q.x * q.z, 1 - 2 * sqy - 2 * sqz) * + // AngleFloat.Rad2Deg, + // MathF.Asin(2 * test) * AngleFloat.Rad2Deg); + // } + } + + /// + /// Create a rotation from euler angles + /// + /// The angle around the right axis + /// The angle around the upward axis + /// The angle around the forward axis + /// The resulting quaternion + /// Rotation are appied in the order Z, X, Y. + public static Quaternion Euler(float x, float y, float z) { + return Quaternion.Euler(new Vector3Float(x, y, z)); + } + /// + /// Create a rotation from a vector containing euler angles + /// + /// Vector with the euler angles + /// The resulting quaternion + /// Rotation are appied in the order Z, X, Y. + public static Quaternion Euler(Vector3Float angles) { + Vector3Float euler = angles * AngleFloat.Deg2Rad; + float cx = MathF.Cos(euler.horizontal * 0.5f); + float sx = MathF.Sin(euler.horizontal * 0.5f); + float cy = MathF.Cos(euler.vertical * 0.5f); + float sy = MathF.Sin(euler.vertical * 0.5f); + float cz = MathF.Cos(euler.depth * 0.5f); + float sz = MathF.Sin(euler.depth * 0.5f); + + // Unity uses intrinsic Z, then X, then Y -> q = Qy * Qx * Qz + Quaternion q; + q.w = cy * cx * cz + sy * sx * sz; + q.x = cy * sx * cz + sy * cx * sz; + q.y = sy * cx * cz - cy * sx * sz; + q.z = cy * cx * sz - sy * sx * cz; + return q; + } + + /// + /// Multiply two quaternions + /// + /// + /// + /// The resulting rotation + public static Quaternion operator *(Quaternion q1, Quaternion q2) { + return new Quaternion( + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x, + -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y, + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z, + -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w); + } + + /// + /// Rotate a vector using this quaterion + /// + /// The rotation + /// The vector to rotate + /// The rotated vector + public static Vector3Float operator *(Quaternion q, Vector3Float v) { + float num = q.x * 2; + float num2 = q.y * 2; + float num3 = q.z * 2; + float num4 = q.x * num; + float num5 = q.y * num2; + float num6 = q.z * num3; + float num7 = q.x * num2; + float num8 = q.x * num3; + float num9 = q.y * num3; + float num10 = q.w * num; + float num11 = q.w * num2; + float num12 = q.w * num3; + + float px = v.horizontal; + float py = v.vertical; + float pz = v.depth; + float rx = + (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz; + float ry = + (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz; + float rz = + (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz; + Vector3Float result = new(rx, ry, rz); + return result; + } + + /// + /// The inverse of quaterion + /// + /// The quaternion for which the inverse is + /// needed The inverted quaternion + public static Quaternion Inverse(Quaternion q) { + float n = MathF.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + return new Quaternion(-q.x / n, -q.y / n, -q.z / n, q.w / n); + } + + /// + /// A rotation which looks in the given direction + /// + /// The look direction + /// The up direction + /// The look rotation + public static Quaternion LookRotation(Vector3Float forward, Vector3Float up) { + Vector3Float nForward = forward.normalized; + Vector3Float nRight = Vector3Float.Normalize(Vector3Float.Cross(up, nForward)); + Vector3Float nUp = Vector3Float.Cross(nForward, nRight); + float m00 = nRight.horizontal; // x; + float m01 = nRight.vertical; // y; + float m02 = nRight.depth; // z; + float m10 = nUp.horizontal; // x; + float m11 = nUp.vertical; // y; + float m12 = nUp.depth; // z; + float m20 = nForward.horizontal; // x; + float m21 = nForward.vertical; // y; + float m22 = nForward.depth; // z; + + float num8 = (m00 + m11) + m22; + float x, y, z, w; + if (num8 > 0) { + float num = MathF.Sqrt(num8 + 1); + w = num * 0.5f; + num = 0.5f / num; + x = (m12 - m21) * num; + y = (m20 - m02) * num; + z = (m01 - m10) * num; + return new Quaternion(x, y, z, w); + } + if ((m00 >= m11) && (m00 >= m22)) { + float num7 = MathF.Sqrt(((1 + m00) - m11) - m22); + float num4 = 0.5F / num7; + x = 0.5f * num7; + y = (m01 + m10) * num4; + z = (m02 + m20) * num4; + w = (m12 - m21) * num4; + return new Quaternion(x, y, z, w); + } + if (m11 > m22) { + float num6 = MathF.Sqrt(((1 + m11) - m00) - m22); + float num3 = 0.5F / num6; + x = (m10 + m01) * num3; + y = 0.5F * num6; + z = (m21 + m12) * num3; + w = (m20 - m02) * num3; + return new Quaternion(x, y, z, w); + } + float num5 = MathF.Sqrt(((1 + m22) - m00) - m11); + float num2 = 0.5F / num5; + x = (m20 + m02) * num2; + y = (m21 + m12) * num2; + z = 0.5F * num5; + w = (m01 - m10) * num2; + return new Quaternion(x, y, z, w); + } + + /// + /// Creates a quaternion with the given forward direction with up = + /// Vector3::up + /// + /// The look direction + /// The rotation for this direction + /// For the rotation, Vector::up is used for the up direction. + /// Note: if the forward direction == Vector3::up, the result is + /// Quaternion::identity + public static Quaternion LookRotation(Vector3Float forward) { + Vector3Float up = new(0, 1, 0); + return LookRotation(forward, up); + } + + /// + /// Calculat the rotation from on vector to another + /// + /// The from direction + /// The to direction + /// The rotation from the first to the second vector + public static Quaternion FromToRotation(Vector3Float fromDirection, Vector3Float toDirection) { + Vector3Float axis = Vector3Float.Cross(fromDirection, toDirection); + axis = axis.normalized; + AngleFloat angle = Vector3Float.SignedAngle(fromDirection, toDirection, axis); + Quaternion rotation = AngleAxis(angle, axis); + return rotation; + + } + + /// + /// Rotate form one orientation to anther with a maximum amount of degrees + /// + /// The from rotation + /// The destination rotation + /// The maximum amount of degrees to + /// rotate The possibly limited rotation + public static Quaternion RotateTowards(Quaternion from, Quaternion to, + float maxDegreesDelta) { + float num = Quaternion.UnsignedAngle(from, to); + if (num == 0) { + return to; + } + float t = MathF.Min(1, maxDegreesDelta / num); + return SlerpUnclamped(from, to, t); + + } + + /// + /// Convert an angle/axis representation to a quaternion + /// + /// The angle + /// The axis + /// The resulting quaternion + public static Quaternion AngleAxis(AngleFloat angle, Vector3Float axis) { + if (axis.sqrMagnitude == 0.0f) + return Quaternion.identity; + + float radians = angle.inRadians; + radians *= 0.5f; + + Vector3Float axis2 = axis * MathF.Sin(radians); + float x = axis2.horizontal; // x; + float y = axis2.vertical; // y; + float z = axis2.depth; // z; + float w = MathF.Cos(radians); + + return new Quaternion(x, y, z, w).normalized; + } + /// + /// Convert this quaternion to angle/axis representation + /// + /// A pointer to the angle for the result + /// A pointer to the axis for the result + public readonly void ToAngleAxis(out AngleFloat angle, out Vector3Float axis) { + Quaternion q1 = (MathF.Abs(this.w) > 1.0f) ? this.normalized : this; + angle = AngleFloat.Radians(2.0f * MathF.Acos(q1.w)); // angle + float den = MathF.Sqrt(1.0F - q1.w * q1.w); + if (den > 0.0001f) { + axis = Vector3Float.Normalize(q1.xyz / den); + } + else { + // This occurs when the angle is zero. + // Not a problem: just set an arbitrary normalized axis. + axis = Vector3Float.right; + } + } + + /// + /// Get the angle between two orientations + /// + /// The first orientation + /// The second orientation + /// The smallest angle in degrees between the two + /// orientations + public static float UnsignedAngle(Quaternion q1, Quaternion q2) { + // float f = Dot(q1, q2); + // return MathF.Acos(MathF.Min(MathF.Abs(f), 1)) * 2 * AngleFloat.Rad2Deg; + + float dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + dot = MathF.Min(MathF.Max(dot, -1f), 1f); + return 2f * MathF.Acos(MathF.Abs(dot)) * (180f / MathF.PI); + } + + /// + /// Sherical lerp between two rotations + /// + /// The first rotation + /// The second rotation + /// The factor between 0 and 1. + /// The resulting rotation + /// A factor 0 returns rotation1, factor1 returns rotation2. + public static Quaternion Slerp(Quaternion a, + Quaternion b, float t) { + if (t > 1) + t = 1; + if (t < 0) + t = 0; + return SlerpUnclamped(a, b, t); + } + + /// + /// Unclamped sherical lerp between two rotations + /// + /// The first rotation + /// The second rotation + /// The factor + /// The resulting rotation + /// A factor 0 returns rotation1, factor1 returns rotation2. + /// Values outside the 0..1 range will result in extrapolated rotations + public static Quaternion SlerpUnclamped(Quaternion a, + Quaternion b, float t) { + // if either input is zero, return the other. + if (a.sqrMagnitude == 0.0f) { + if (b.sqrMagnitude == 0.0f) { + return identity; + } + return b; + } + else if (b.sqrMagnitude == 0.0f) { + return a; + } + + Vector3Float axyz = a.xyz; + Vector3Float bxyz = b.xyz; + float cosHalfAngle = a.w * b.w + Vector3Float.Dot(axyz, bxyz); + + Quaternion b2 = b; + if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { + // angle = 0.0f, so just return one input. + return a; + } + else if (cosHalfAngle < 0.0f) { + b2.x = -b.x; + b2.y = -b.y; + b2.z = -b.z; + b2.w = -b.w; + cosHalfAngle = -cosHalfAngle; + } + + float blendA; + float blendB; + if (cosHalfAngle < 0.99f) { + // do proper slerp for big angles + float halfAngle = MathF.Acos(cosHalfAngle); + float sinHalfAngle = MathF.Sin(halfAngle); + float oneOverSinHalfAngle = 1.0F / sinHalfAngle; + blendA = MathF.Sin(halfAngle * (1.0F - t)) * oneOverSinHalfAngle; + blendB = MathF.Sin(halfAngle * t) * oneOverSinHalfAngle; + } + else { + // do lerp if angle is really small. + blendA = 1.0f - t; + blendB = t; + } + Vector3Float v = axyz * blendA + b2.xyz * blendB; + Quaternion result = + new(v.horizontal, v.vertical, v.depth, blendA * a.w + blendB * b2.w); + if (result.sqrMagnitude > 0.0f) + return result.normalized; + else + return Quaternion.identity; + } + + /// + /// Convert this quaternion to angle/axis representation + /// + /// A pointer to the angle for the result + /// A pointer to the axis for the result + public readonly void ToAngleAxis(out float angle, out Vector3Float axis) { + ToAxisAngleRad(this, out axis, out angle); + angle *= AngleFloat.Rad2Deg; + } + private static void ToAxisAngleRad(Quaternion q, + out Vector3Float axis, + out float angle) { + Quaternion q1 = (MathF.Abs(q.w) > 1.0f) ? Quaternion.Normalize(q) : q; + angle = 2.0f * MathF.Acos(q1.w); // angle + float den = MathF.Sqrt(1.0F - q1.w * q1.w); + if (den > 0.0001f) { + axis = (q1.xyz / den).normalized; + } + else { + // This occurs when the angle is zero. + // Not a problem: just set an arbitrary normalized axis. + axis = new Vector3Float(1, 0, 0); + } + } + + /// + /// Returns the angle of around the give axis for a rotation + /// + /// The axis around which the angle should be + /// computed The source rotation + /// The signed angle around the axis + public static float GetAngleAround(Vector3Float axis, Quaternion rotation) { + Quaternion secondaryRotation = GetRotationAround(axis, rotation); + secondaryRotation.ToAngleAxis(out float rotationAngle, out Vector3Float rotationAxis); + + // Do the axis point in opposite directions? + if (Vector3Float.Dot(axis, rotationAxis) < 0) + rotationAngle = -rotationAngle; + + return rotationAngle; + } + + /// + /// Returns the rotation limited around the given axis + /// + /// The axis which which the rotation should be + /// limited The source rotation + /// The rotation around the given axis + public static Quaternion GetRotationAround(Vector3Float axis, Quaternion rotation) { + Vector3Float ra = new(rotation.x, rotation.y, rotation.z); // rotation axis + Vector3Float p = Vector3Float.Project( + ra, axis); // return projection ra on to axis (parallel component) + Quaternion twist = new(p.horizontal, p.vertical, p.depth, rotation.w); + twist = Normalize(twist); + return twist; + + } + + /// + /// Swing-twist decomposition of a rotation + /// + /// The base direction for the decomposition + /// The source rotation + /// A pointer to the quaternion for the swing + /// result A pointer to the quaternion for the + /// twist result + static void GetSwingTwist(Vector3Float axis, Quaternion q, + out Quaternion swing, out Quaternion twist) { + twist = GetRotationAround(axis, q); + swing = q * Inverse(twist); + } + + /// + /// Calculate the dot product of two quaternions + /// + /// The first rotation + /// The second rotation + /// + public static float Dot(Quaternion q1, Quaternion q2) { + return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; + } + } +#endif + +} \ No newline at end of file diff --git a/LinearAlgebra/src/Spherical.cs b/LinearAlgebra/src/Spherical.cs new file mode 100644 index 0000000..318839d --- /dev/null +++ b/LinearAlgebra/src/Spherical.cs @@ -0,0 +1,279 @@ +using System; +using System.Collections.Generic; + +#if UNITY_5_3_OR_NEWER +using Vector3 = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra { + /// + /// A spherical vector + /// + /// This is a struct such that it is a value type and cannot be null + public struct Spherical { + /// + /// Create a spherical vector + /// + /// The distance in meters + /// The direction of the vector + public Spherical(float distance, Direction direction) { + if (distance > 0) { + this.distance = distance; + this.direction = direction; + } + else { + this.distance = -distance; + this.direction = -direction; + } + } + + /// + /// Create spherical vector. All given angles are in degrees + /// + /// The distance in meters + /// The horizontal angle in degrees + /// The vertical angle in degrees + /// + public static Spherical Degrees(float distance, float horizontal, float vertical) { + Direction direction = Direction.Degrees(horizontal, vertical); + Spherical s = new(distance, direction); + return s; + } + + public static Spherical Radians(float distance, float horizontal, float vertical) { + Direction direction = Direction.Radians(horizontal, vertical); + Spherical s = new(distance, direction); + return s; + } + + /// + /// The distance in meters + /// + /// @remark The distance should never be negative + public float distance; + /// + /// The direction of the vector + /// + public Direction direction; + + /// + /// A spherical vector with zero degree angles and distance + /// + public readonly static Spherical zero = new(0, Direction.forward); + /// + /// A normalized forward-oriented vector + /// + public readonly static Spherical forward = new(1, Direction.forward); + +#if UNITY_5_3_OR_NEWER + public static Spherical FromVector3(Vector3 v) { + float distance = v.magnitude; + Direction direction = Direction.FromVector3(v / distance); + return new Spherical(distance, direction); + } + + public readonly Vector3 ToVector3() { + Vector3 v = this.direction.ToVector3(); + v *= this.distance; + return v; + } +#else + public static Spherical FromVector3(Vector3Float v) { + float distance = v.magnitude; + if (distance == 0.0f) + return Spherical.zero; + else { + float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.vertical / distance)) * AngleFloat.Rad2Deg; + float horizontalAngle = (float)Math.Atan2(v.horizontal, v.depth) * AngleFloat.Rad2Deg; + return Degrees(distance, horizontalAngle, verticalAngle); + } + } + + public readonly Vector3Float ToVector3() { + // float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians; + // float horizontalRad = this.direction.horizontal.inRadians; + // float cosVertical = (float)Math.Cos(verticalRad); + // float sinVertical = (float)Math.Sin(verticalRad); + // float cosHorizontal = (float)Math.Cos(horizontalRad); + // float sinHorizontal = (float)Math.Sin(horizontalRad); + + // float x = this.distance * sinVertical * sinHorizontal; + // float y = this.distance * cosVertical; + // float z = this.distance * sinVertical * cosHorizontal; + + // Vector3Float v = new(x, y, z); + Vector3Float v = this.direction.ToVector3(); + v *= this.distance; + return v; + } +#endif + + public override readonly string ToString() { + return $"Spherical({this.distance}, h: {this.direction.horizontal}, v: {this.direction.vertical})"; + } + + + public readonly float magnitude => this.distance; + + public Spherical normalized { + get { + Spherical r = new() { + distance = 1, + direction = this.direction + }; + return r; + } + } + + public static Spherical operator +(Spherical s1, Spherical s2) { + // let's do it the easy way... + // using vars to be compatible with both unity (Vector3) and native (Vector3Float) + var v1 = s1.ToVector3(); + var v2 = s2.ToVector3(); + var v = v1 + v2; + Spherical r = FromVector3(v); + return r; + } + + public static Spherical operator *(Spherical v, float d) { + Spherical r = new(v.distance * d, v.direction); + return r; + } + + public static bool operator ==(Spherical v1, Spherical v2) { + return (v1.distance == v2.distance && v1.direction == v2.direction); + } + + public static bool operator !=(Spherical v1, Spherical v2) { + return (v1.distance != v2.distance || v1.direction != v2.direction); + } + + public override readonly bool Equals(object o) { + if (o is Spherical s) + return this == s; + return false; + } + + public override readonly int GetHashCode() { + return HashCode.Combine(this.distance, this.direction); + } + + public static float Distance(Spherical v1, Spherical v2) { + // Convert degrees to radians + float thetaARadians = v1.direction.horizontal.inRadians; + float phiARadians = v1.direction.vertical.inRadians;// DegreesToRadians(phiA); + float thetaBRadians = v2.direction.horizontal.inRadians; // DegreesToRadians(thetaB); + float phiBRadians = v2.direction.vertical.inRadians; // DegreesToRadians(phiB); + + // Calculate sine and cosine values + float sinPhiA = MathF.Sin(phiARadians); + float cosPhiA = MathF.Cos(phiARadians); + float sinPhiB = MathF.Sin(phiBRadians); + float cosPhiB = MathF.Cos(phiBRadians); + + // Calculate the cosine of the difference in azimuthal angles + float cosThetaDifference = MathF.Cos(thetaARadians - thetaBRadians); + + // Apply the spherical law of cosines + float distance = MathF.Sqrt( + v1.distance * v1.distance + + v2.distance * v2.distance - + 2 * v1.distance * v2.distance * (sinPhiA * sinPhiB * cosThetaDifference + cosPhiA * cosPhiB) + ); + + return distance; + } + + public static Spherical Average(Spherical v1, Spherical v2) { + const float EPS = 1e-6f; + + // Angles in radians + float a1 = v1.direction.horizontal.inRadians; + float a2 = v2.direction.horizontal.inRadians; + float e1 = v1.direction.vertical.inRadians; + float e2 = v2.direction.vertical.inRadians; + + // Fast path: exactly same direction (allowing wrap for azimuth) -> preserve exact angles + bool sameAz = MathF.Abs(MathF.IEEERemainder(a1 - a2, MathF.PI * 2f)) < EPS; + bool sameEl = MathF.Abs(e1 - e2) < EPS; + if (sameAz && sameEl) { + // Distances may differ; average distance but keep exact angles from v1 + float rAvgExact = 0.5f * (v1.distance + v2.distance); + return new Spherical(rAvgExact, v1.direction); + } + + // Horizontal unit-circle sum + float cx = MathF.Cos(a1) + MathF.Cos(a2); + float cy = MathF.Sin(a1) + MathF.Sin(a2); + + // Vertical as z = sin(el) + float z1 = MathF.Sin(e1); + float z2 = MathF.Sin(e2); + float cz = z1 + z2; + + // Magnitude of summed unit-direction vectors + float sumX = cx; + float sumY = cy; + float sumZ = cz; + float magSum = MathF.Sqrt(sumX * sumX + sumY * sumY + sumZ * sumZ); + + // If the two direction unit-vectors cancel (or nearly), return zero distance. + if (magSum < EPS) { + return Spherical.Radians(0f, 0f, 0f); + } + + // Normalized averaged direction components + float ux = sumX / magSum; + float uy = sumY / magSum; + float uz = sumZ / magSum; + + // Compute averaged angles from normalized vector + float azAvgRad = MathF.Atan2(uy, ux); + float elAvgRad = MathF.Asin(Float.Clamp(uz, -1f, 1f)); + + // Average distance (arithmetic mean) + float rAvg = 0.5f * (v1.distance + v2.distance); + + return Spherical.Radians(rAvg, azAvgRad, elAvgRad); + } + + public static Spherical Sum(List vectors) { + if (vectors == null || vectors.Count == 0) + throw new ArgumentException("vectors must contain at least one element", nameof(vectors)); + +#if UNITY_5_3_OR_NEWER + Vector3 sum = Vector3.zero; +#else + Vector3Float sum = Vector3Float.zero; +#endif + foreach (Spherical v in vectors) + sum += v.ToVector3(); + + return FromVector3(sum); + } + + + public static Spherical Average(List vectors) { + if (vectors == null || vectors.Count == 0) + throw new ArgumentException("vectors must contain at least one element", nameof(vectors)); + +#if UNITY_5_3_OR_NEWER + Vector3 sum = Vector3.zero; +#else + Vector3Float sum = Vector3Float.zero; +#endif + int n = 0; + foreach (Spherical v in vectors) { + sum += v.ToVector3(); + n++; + } + var avg = sum / n; + + // if (avg.sqrMagnitude == 0f) + // return new Spherical(0f, new Direction(AngleFloat.Radians(0f), AngleFloat.Radians(0f))); + // else + return FromVector3(avg); + } + + } +} \ No newline at end of file diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs new file mode 100644 index 0000000..df6e048 --- /dev/null +++ b/LinearAlgebra/src/SwingTwist.cs @@ -0,0 +1,136 @@ +// #if !UNITY_5_3_OR_NEWER +// using UnityEngine; +// #endif + +namespace LinearAlgebra { + + /// + /// An orientation using swing and twist angles + /// + /// The swing rotation + /// The twist rotation + public struct SwingTwist { + public Direction swing; + public AngleFloat twist; + + public SwingTwist(Direction swing, AngleFloat twist) { + this.swing = swing; + this.twist = twist; + } + + /// + /// Create a swing/twist rotation using angles in degrees + /// + /// The swing angle in the horizontal plane in degrees + /// The swing angle in the vertical plan in degrees + /// The twist angle in degrees + /// The swing/twist rotation + public static SwingTwist Degrees(float horizontalSwing, float verticalSwing, float twist) { + Direction swing = Direction.Degrees(horizontalSwing, verticalSwing); + AngleFloat twistAngle = AngleFloat.Degrees(twist); + SwingTwist s = new(swing, twistAngle); + return s; + } + + /// + /// Create a swing/twist rotation using angles in degrees + /// + /// The swing angle in the horizontal plane in degrees + /// The swing angle in the vertical plan in degrees + /// The twist angle in degrees + /// The swing/twist rotation + public static SwingTwist Radians(float horizontalSwing, float verticalSwing, float twist) { + Direction swing = Direction.Radians(horizontalSwing, verticalSwing); + AngleFloat twistAngle = AngleFloat.Radians(twist); + SwingTwist s = new(swing, twistAngle); + return s; + } + +#if UNITY_5_3_OR_NEWER + /// + /// A zero angle rotation + /// + public static readonly SwingTwist zero = Degrees(0, 0, 0); + + public Spherical ToAngleAxis() { + UnityEngine.Quaternion q = this.ToQuaternion(); + q.ToAngleAxis(out float angle, out UnityEngine.Vector3 axis); + Direction direction = Direction.FromVector3(axis); + + Spherical r = new(angle, direction); + return r; + } + + public static SwingTwist FromAngleAxis(Spherical r) { + UnityEngine.Vector3 vectorAxis = r.direction.ToVector3(); + UnityEngine.Quaternion q = UnityEngine.Quaternion.AngleAxis(r.distance, vectorAxis); + return FromQuaternion(q); + } + + /// + /// Convert a quaternion in a swing/twist rotation + /// + /// The quaternion to convert + /// The swing/twist rotation + public static SwingTwist FromQuaternion(UnityEngine.Quaternion q) { + UnityEngine.Vector3 angles = q.eulerAngles; + SwingTwist r = Degrees(angles.y, -angles.x, -angles.z); + return r; + } + + public UnityEngine.Quaternion ToQuaternion() { + UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(this.swing.vertical.inDegrees, + this.swing.horizontal.inDegrees, + this.twist.inDegrees); + return q; + } +#else + /// + /// A zero angle rotation + /// + public static readonly SwingTwist zero = Degrees(0, 0, 0); + + public Spherical ToAngleAxis() { + LinearAlgebra.Quaternion q = this.ToQuaternion(); + q.ToAngleAxis(out float angle, out Vector3Float axis); + Direction direction = Direction.FromVector3(axis); + + Spherical r = new(angle, direction); + return r; + } + + public static SwingTwist FromAngleAxis(Spherical r) { + Vector3Float vectorAxis = r.direction.ToVector3(); + LinearAlgebra.Quaternion q = LinearAlgebra.Quaternion.AngleAxis(AngleFloat.Degrees(r.distance), vectorAxis); + return FromQuaternion(q); + } + + /// + /// Convert a quaternion in a swing/twist rotation + /// + /// The quaternion to convert + /// The swing/twist rotation + public static SwingTwist FromQuaternion(LinearAlgebra.Quaternion q) { + Vector3Float v = LinearAlgebra.Quaternion.ToAngles(q); + SwingTwist r = Degrees(v.vertical, v.horizontal, v.depth); + return r; + } + + public LinearAlgebra.Quaternion ToQuaternion() { + LinearAlgebra.Quaternion q = LinearAlgebra.Quaternion.Euler(this.swing.vertical.inDegrees, + this.swing.horizontal.inDegrees, + this.twist.inDegrees); + return q; + + } + + public static SwingTwist FromQuat32(Quat32 q32) { + q32.ToAngles(out float right, out float up, out float forward); + SwingTwist r = Degrees(up, right, forward); + return r; + } +#endif + + } + +} \ No newline at end of file diff --git a/LinearAlgebra/src/Vector2Float.cs b/LinearAlgebra/src/Vector2Float.cs new file mode 100644 index 0000000..ac1867c --- /dev/null +++ b/LinearAlgebra/src/Vector2Float.cs @@ -0,0 +1,479 @@ +using System; +using System.Numerics; + +namespace LinearAlgebra { + + /* + public struct Vector2Int { + public int horizontal; + public int vertical; + + public Vector2Int(int horizontal, int vertical) { + this.horizontal = horizontal; + this.vertical = vertical; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2Int zero = new(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2Int one = new(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2Int up = new(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2Int down = new(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2Int forward = new(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2Int back = new(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2Int left = new(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2Int right = new(1, 0); + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical; + + /// + /// Tests if the vector is equal to the given object + /// + /// The object to compare to + /// false when the object is not a Vector2 or does not have equal values + public override readonly bool Equals(object obj) { + if (obj is not Vector2Int v) + return false; + + return (this.horizontal == v.horizontal && this.vertical == v.vertical); + } + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// Note that this uses a Float equality check which cannot be not exact in all cases. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator ==(Vector2Int v1, Vector2Int v2) { + return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical); + } + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// Note that this uses a Float equality check which cannot be not exact in all case. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon. + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator !=(Vector2Int v1, Vector2Int v2) { + return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical); + } + public readonly float magnitude { + get { + int h = this.horizontal; + int v = this.vertical; + return MathF.Sqrt(h * h + v * v); + } + } + + public static float MagnitudeOf(Vector2Int v) { + return v.magnitude; + } + + public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical); + } + public static Vector2Int operator +(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical); + } + + public static float Distance(Vector2Int v1, Vector2Int v2) { + return (v1 - v2).magnitude; + } + } + + public struct Vector2Float { + public float horizontal; + public float vertical; + + public Vector2Float(float horizontal, float vertical) { + this.horizontal = horizontal; + this.vertical = vertical; + } + + public readonly float magnitude { + get { + float h = this.horizontal; + float v = this.vertical; + return MathF.Sqrt(h * h + v * v); + } + } + + public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) { + return new Vector2Float(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical); + } + + public static float Distance(Vector2Float v1, Vector2Float v2) { + return (v1 - v2).magnitude; + } + } + */ + + /// + /// 2-dimensional vectors + /// + public struct Vector2Float { + + /// + /// The right axis of the vector + /// + public float horizontal; // left/right + /// + /// The upward/forward axis of the vector + /// + public float vertical; // forward/backward + // directions are to be inline with Vector3 as much as possible... + + /// + /// Create a new 2-dimensional vector + /// + /// x axis value + /// y axis value + public Vector2Float(float x, float y) { + this.horizontal = x; + this.vertical = y; + } + + /// + /// Convert a Vector2Int into a Vector2Float + /// + /// The Vector2Int + public Vector2Float(Vector2Int v) { + this.horizontal = v.horizontal; + this.vertical = v.vertical; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2Float zero = new Vector2Float(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2Float one = new Vector2Float(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2Float up = new Vector2Float(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2Float down = new Vector2Float(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2Float forward = new Vector2Float(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2Float back = new Vector2Float(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2Float left = new Vector2Float(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2Float right = new Vector2Float(1, 0); + + /// + /// The squared length of this vector + /// + /// The squared length + /// The squared length is computationally simpler than the real length. + /// Think of Pythagoras A^2 + B^2 = C^2. + /// This leaves out the calculation of the squared root of C. + public readonly float sqrMagnitude => horizontal * horizontal + vertical * vertical; + public static float SqrMagnitudeOf(Vector2Float v) { + return v.sqrMagnitude; + } + + /// + /// The length of this vector + /// + /// The length of this vector + public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical); + public static float MagnitudeOf(Vector2Float v) { + return v.magnitude; + } + + /// + /// Convert the vector to a length of a 1 + /// + /// The vector with length 1 + public Vector2Float normalized { + get { + float l = magnitude; + Vector2Float v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + public static Vector2Float Normalize(Vector2Float v) { + return v.normalized; + } + + /// + /// Add two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2Float operator +(Vector2Float v1, Vector2Float v2) { + Vector2Float v = new Vector2Float(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical); + return v; + } + + /// + /// Subtract two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) { + Vector2Float v = new Vector2Float(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical); + return v; + } + + /// + /// Negate the vector + /// + /// The vector to negate + /// The negated vector + /// This will result in a vector pointing in the opposite direction + public static Vector2Float operator -(Vector2Float v1) { + Vector2Float v = new Vector2Float(-v1.horizontal, -v1.vertical); + return v; + } + + /// + /// Scale a vector uniformly down + /// + /// The vector to scale + /// The scaling factor + /// The scaled vector + /// Each component of the vector will be devided by the same factor. + public static Vector2Float operator /(Vector2Float v, float f) { + Vector2Float r = new(v.horizontal / f, v.vertical / f); + return r; + } + + + /// + /// Scale a vector uniformly up + /// + /// The vector to scale + /// The scaling factor + /// The scaled vector + /// Each component of the vector will be multipled with the same factor. + public static Vector2Float operator *(Vector2Float v1, float f) { + Vector2Float v = new Vector2Float(v1.horizontal * f, v1.vertical * f); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The scaling factor + /// The vector to scale + /// The scaled vector + /// Each component of the vector will be multipled with the same factor. + public static Vector2Float operator *(float f, Vector2Float v1) { + Vector2Float v = new Vector2Float(f * v1.horizontal, f * v1.vertical); + return v; + } + + /// @brief Scale the vector using another vector + /// @param v1 The vector to scale + /// @param v2 A vector with the scaling factors + /// @return The scaled vector + /// @remark Each component of the vector v1 will be multiplied with the + /// matching component from the scaling vector v2. + public static Vector2Float Scale(Vector2Float v1, Vector2Float v2) { + return new Vector2Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical); + } + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + //public readonly bool Equals(Vector2Float v1) => horizontal == v1.horizontal && vertical == v1.vertical; + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// Note that this uses a Float equality check which cannot be not exact in all cases. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator ==(Vector2Float v1, Vector2Float v2) { + return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical); + } + + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// Note that this uses a Float equality check which cannot be not exact in all case. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon. + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator !=(Vector2Float v1, Vector2Float v2) { + return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical); + } + + /// + /// Tests if the vector is equal to the given object + /// + /// The object to compare to + /// false when the object is not a Vector2 or does not have equal values + public override readonly bool Equals(object obj) { + if (obj is not Vector2Float v) + return false; + + return (horizontal == v.horizontal && vertical == v.vertical); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override readonly int GetHashCode() { + return HashCode.Combine(horizontal, vertical); + } + + /// + /// Get the distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + public static float Distance(Vector2Float v1, Vector2Float v2) { + float x = v1.horizontal - v2.horizontal; + float y = v1.vertical - v2.vertical; + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + + /// + /// The dot product of two vectors + /// + /// The first vector + /// The second vector + /// The dot product of the two vectors + public static float Dot(Vector2Float v1, Vector2Float v2) { + return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical; + } + + /// + /// Calculate the signed angle between two vectors. + /// + /// The starting vector + /// The ending vector + /// The axis to rotate around + /// The signed angle in degrees + public static float SignedAngle(Vector2Float from, Vector2Float to) { + //float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y); + //return Vector2.Angle(v1, v2) * sign; + + float sqrMagFrom = from.sqrMagnitude; + float sqrMagTo = to.sqrMagnitude; + + if (sqrMagFrom == 0 || sqrMagTo == 0) + return 0; + //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo)) + // return nanf(""); + + float angleFrom = (float)Math.Atan2(from.vertical, from.horizontal); + float angleTo = (float)Math.Atan2(to.vertical, to.horizontal); + return -(angleTo - angleFrom) * AngleFloat.Rad2Deg; + } + + public static float UnsignedAngle(Vector2Float from, Vector2Float to) { + return MathF.Abs(SignedAngle(from, to)); + } + + /// + /// Rotates the vector with the given angle + /// + /// The vector to rotate + /// The angle in degrees + /// + public static Vector2Float Rotate(Vector2Float v1, AngleFloat angle) { + float sin = (float)Math.Sin(angle.inRadians); + float cos = (float)Math.Cos(angle.inRadians); + // float sin = AngleFloat.Sin(angle); + // float cos = AngleFloat.Cos(angle); + + float tx = v1.horizontal; + float ty = v1.vertical; + Vector2Float v = new Vector2Float() { + horizontal = (cos * tx) - (sin * ty), + vertical = (sin * tx) + (cos * ty) + }; + return v; + } + + /// + /// Lerp between two vectors + /// + /// The from vector + /// The to vector + /// The interpolation distance [0..1] + /// The lerped vector + /// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1 + /// matches the *v2* vector Value -1 is *v1* vector minus the difference + /// between *v1* and *v2* etc. + public static Vector2Float Lerp(Vector2Float v1, Vector2Float v2, float f) { + Vector2Float v = v1 + (v2 - v1) * f; + return v; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// Vectors a and b must be normalized + public static float ToFactor(Vector2Float v1, Vector2Float v2) { + return (1 - Vector2Float.Dot(v1, v2)) / 2; + } + } +} \ No newline at end of file diff --git a/LinearAlgebra/src/Vector2Int.cs b/LinearAlgebra/src/Vector2Int.cs new file mode 100644 index 0000000..ed68e8b --- /dev/null +++ b/LinearAlgebra/src/Vector2Int.cs @@ -0,0 +1,185 @@ +using System; + +namespace LinearAlgebra { + + public struct Vector2Int { + public int horizontal; + public int vertical; + + public Vector2Int(int horizontal, int vertical) { + this.horizontal = horizontal; + this.vertical = vertical; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2Int zero = new(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2Int one = new(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2Int up = new(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2Int down = new(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2Int forward = new(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2Int back = new(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2Int left = new(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2Int right = new(1, 0); + + /* + /// + /// Get an hash code for the vector + /// + /// The hash code + public override int GetHashCode() { + return (this.horizontal, this.vertical).GetHashCode(); + } + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + public readonly bool Equals(Vector2Int v) => this.horizontal == v.horizontal && vertical == v.vertical; + + */ + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// Note that this uses a Float equality check which cannot be not exact in all cases. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator ==(Vector2Int v1, Vector2Int v2) { + return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical); + } + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// Note that this uses a Float equality check which cannot be not exact in all case. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon. + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator !=(Vector2Int v1, Vector2Int v2) { + return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical); + } + + /// + /// Tests if the vector is equal to the given object + /// + /// The object to compare to + /// false when the object is not a Vector2 or does not have equal values + public override readonly bool Equals(object obj) { + if (obj is not Vector2Int v) + return false; + + return (this.horizontal == v.horizontal && this.vertical == v.vertical); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override readonly int GetHashCode() { + return HashCode.Combine(horizontal, vertical); + } + + public readonly float sqrMagnitude => this.horizontal * this.horizontal + this.vertical * this.vertical; + + public static float SqrMagnitudeOf(Vector2Int v) { + return v.sqrMagnitude; + } + + public readonly float magnitude => + MathF.Sqrt(this.horizontal * this.horizontal + this.vertical * this.vertical); + + public static float MagnitudeOf(Vector2Int v) { + return v.magnitude; + } + + /// @brief Convert the vector to a length of 1 + /// @return The vector normalized to a length of 1 + public readonly Vector2Float normalized { + get { + float l = magnitude; + Vector2Float v = Vector2Float.zero; + if (l > Float.epsilon) + v = new Vector2Float(this) / l; + return v; + } + } + /// @brief Convert the vector to a length of 1 + /// @param v The vector to convert + /// @return The vector normalized to a length of 1 + public static Vector2Float Normalize(Vector2Int v) { + float num = v.magnitude; + Vector2Float result = Vector2Float.zero; + if (num > Float.epsilon) + result = new Vector2Float(v) / num; + + return result; + } + + public static Vector2Int operator -(Vector2Int v) { + return new Vector2Int(-v.horizontal, -v.vertical); + } + + public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical); + } + public static Vector2Int operator +(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical); + } + + public static Vector2Int operator /(Vector2Int v, int f) { + return new Vector2Int(v.horizontal / f, v.vertical / f); + } + + public static Vector2Int operator *(Vector2Int v1, int d) { + return new Vector2Int(v1.horizontal * d, v1.vertical * d); + } + + public static Vector2Int operator *(int d, Vector2Int v1) { + return new Vector2Int(d * v1.horizontal, d * v1.vertical); + } + + public static Vector2Int Scale(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical); + } + + /// @brief The dot product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The dot product of the two vectors + public static int Dot(Vector2Int v1, Vector2Int v2) { + return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical; + } + + public static float Distance(Vector2Int v1, Vector2Int v2) { + return (v1 - v2).magnitude; + } + } +} \ No newline at end of file diff --git a/LinearAlgebra/src/Vector3Float.cs b/LinearAlgebra/src/Vector3Float.cs new file mode 100644 index 0000000..bcf8626 --- /dev/null +++ b/LinearAlgebra/src/Vector3Float.cs @@ -0,0 +1,402 @@ +//#if !UNITY_5_3_OR_NEWER +using System; + +namespace LinearAlgebra { + /* + public struct Vector3Float { + public float horizontal; + public float vertical; + public float depth; + + public Vector3Float(float horizontal, float vertical, float depth) { + this.horizontal = horizontal; + this.vertical = vertical; + this.depth = depth; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3Float zero = new(0, 0, 0); + + public readonly float magnitude { + get => (float)Math.Sqrt(this.horizontal * this.horizontal + this.vertical * this.vertical + this.depth * this.depth); + } + + /// + /// Convert the vector to a length of a 1 + /// + /// The vector with length 1 + public readonly Vector3Float normalized { + get { + float l = magnitude; + Vector3Float v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + + public static Vector3Float operator *(Vector3Float v, float f) { + Vector3Float r = new(v.horizontal * f, v.vertical * f, v.depth * f); + return r; + } + public static Vector3Float operator /(Vector3Float v, float f) { + Vector3Float r = new(v.horizontal / f, v.vertical / f, v.depth / f); + return r; + } + + public static float Dot(Vector3Float v1, Vector3Float v2) { + return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + + v1.depth * v2.depth; + } + + const float epsilon = 1E-05f; + public static Vector3Float Project(Vector3Float v, Vector3Float n) { + float sqrMagnitude = Dot(n, n); + if (sqrMagnitude < epsilon) + return zero; + else { + float dot = Dot(v, n); + Vector3Float r = n * dot; + r /= sqrMagnitude; + return r; + } + + } + } + */ + + /// + /// 3-dimensional vectors + /// + /// This uses the right-handed coordinate system. + public struct Vector3Float { + + /// + /// The right axis of the vector + /// + public float horizontal; //> left/right + /// + /// The upward axis of the vector + /// + public float vertical; //> up/down + /// + /// The forward axis of the vector + /// + public float depth; //> forward/backward + + /// + /// Create a new 3-dimensional vector + /// + /// x axis value + /// y axis value + /// z axis value + public Vector3Float(float horizontal, float vertical, float depth) { + this.horizontal = horizontal; + this.vertical = vertical; + this.depth = depth; + } + + public Vector3Float(Vector3Int v) { + this.horizontal = v.horizontal; + this.vertical = v.vertical; + this.depth = v.depth; + } + + public static Vector3Float FromSpherical(Spherical s) { + float verticalRad = (AngleFloat.deg90 - s.direction.vertical).inRadians; + float horizontalRad = s.direction.horizontal.inRadians; + float cosVertical = MathF.Cos(verticalRad); + float sinVertical = MathF.Sin(verticalRad); + float cosHorizontal = MathF.Cos(horizontalRad); + float sinHorizontal = MathF.Sin(horizontalRad); + + float horizontal = s.distance * sinVertical * sinHorizontal; + float vertical = s.distance * cosVertical; + float depth = s.distance * sinVertical * cosHorizontal; + return new Vector3Float(horizontal, vertical, depth); + } + + public override string ToString() { + return $"({this.horizontal}, {this.vertical}, {this.depth})"; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3Float zero = new Vector3Float(0, 0, 0); + /// + /// A vector with one for all axis + /// + public static readonly Vector3Float one = new Vector3Float(1, 1, 1); + /// + /// A Vector3Float with values (-1, 0, 0) + /// + public static readonly Vector3Float left = new Vector3Float(-1, 0, 0); + /// + /// A vector with values (1, 0, 0) + /// + public static readonly Vector3Float right = new Vector3Float(1, 0, 0); + /// + /// A vector with values (0, -1, 0) + /// + public static readonly Vector3Float down = new Vector3Float(0, -1, 0); + /// + /// A vector with values (0, 1, 0) + /// + public static readonly Vector3Float up = new Vector3Float(0, 1, 0); + /// + /// A vector with values (0, 0, -1) + /// + public static readonly Vector3Float back = new Vector3Float(0, -1, 0); + /// + /// A vector with values (0, 0, 1) + /// + public static readonly Vector3Float forward = new Vector3Float(0, 1, 0); + + /// @brief The vector length + /// @return The vector length + public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical + depth * depth); + /// + /// The vector length + /// + /// The vector for which you need the length + /// The vector length + public static float MagnitudeOf(Vector3Float v) { + return v.magnitude; + } + + /// @brief The squared vector length + /// @return The squared vector length + /// @remark The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the + /// calculation of the squared root of C. + public readonly float sqrMagnitude => (horizontal * horizontal + vertical * vertical + depth * depth); + + /// + /// The squared vector length + /// + /// The vector for which you need the squared length + /// The squared vector length + /// The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the + /// calculation of the squared root of C. + public static float SqrMagnitudeOf(Vector3Float v) { + return v.sqrMagnitude; + } + + /// @brief Convert the vector to a length of 1 + /// @return The vector normalized to a length of 1 + public readonly Vector3Float normalized { + get { + float l = magnitude; + Vector3Float v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + /// @brief Convert the vector to a length of 1 + /// @param v The vector to convert + /// @return The vector normalized to a length of 1 + public static Vector3Float Normalize(Vector3Float v) { + float num = v.magnitude; + Vector3Float result = zero; + if (num > Float.epsilon) + result = v / num; + + return result; + } + + /// + /// Negate te vector such that it points in the opposite direction + /// + /// + /// The negated vector + public static Vector3Float operator -(Vector3Float v1) { + Vector3Float v = new(-v1.horizontal, -v1.vertical, -v1.depth); + return v; + } + + /// + /// Subtract two vectors + /// + /// + /// + /// The result of the subtraction + public static Vector3Float operator -(Vector3Float v1, Vector3Float v2) { + Vector3Float v = new(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical, v1.depth - v2.depth); + return v; + } + + /// + /// Add two vectors + /// + /// + /// + /// The result of the addition + public static Vector3Float operator +(Vector3Float v1, Vector3Float v2) { + Vector3Float v = new(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical, v1.depth + v2.depth); + return v; + } + + /// @brief Scale the vector using another vector + /// @param v1 The vector to scale + /// @param v2 A vector with the scaling factors + /// @return The scaled vector + /// @remark Each component of the vector v1 will be multiplied with the + /// matching component from the scaling vector v2. + public static Vector3Float Scale(Vector3Float v1, Vector3Float v2) { + return new Vector3Float(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical, v1.depth * v2.depth); + } + + + public static Vector3Float operator *(Vector3Float v1, float d) { + Vector3Float v = new(v1.horizontal * d, v1.vertical * d, v1.depth * d); + return v; + } + + public static Vector3Float operator *(float d, Vector3Float v1) { + Vector3Float v = new(d * v1.horizontal, d * v1.vertical, d * v1.depth); + return v; + } + + public static Vector3Float operator /(Vector3Float v1, float d) { + Vector3Float v = new(v1.horizontal / d, v1.vertical / d, v1.depth / d); + return v; + } + + + //public bool Equals(Vector3Float v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); + + public static bool operator ==(Vector3Float v1, Vector3Float v2) { + return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth); + } + + public static bool operator !=(Vector3Float v1, Vector3Float v2) { + return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth); + } + + public override readonly bool Equals(object obj) { + if (obj is not Vector3Float v) + return false; + + return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); + } + + public override readonly int GetHashCode() { + return HashCode.Combine(horizontal, vertical, depth); + } + + /// @brief The distance between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The distance between the two vectors + public static float Distance(Vector3Float v1, Vector3Float v2) { + return (v2 - v1).magnitude; + } + + /// @brief The dot product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The dot product of the two vectors + public static float Dot(Vector3Float v1, Vector3Float v2) { + return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + v1.depth * v2.depth; + } + + /// @brief The cross product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The cross product of the two vectors + public static Vector3Float Cross(Vector3Float v1, Vector3Float v2) { + return new Vector3Float(v1.vertical * v2.depth - v1.depth * v2.vertical, v1.depth * v2.horizontal - v1.horizontal * v2.depth, + v1.horizontal * v2.vertical - v1.vertical * v2.horizontal); + + } + + /// @brief Project the vector on another vector + /// @param v The vector to project + /// @param n The normal vecto to project on + /// @return The projected vector + public static Vector3Float Project(Vector3Float v, Vector3Float n) { + float sqrMagnitude = Dot(n, n); + if (sqrMagnitude < Float.epsilon) + return zero; + else { + float dot = Dot(v, n); + Vector3Float r = n * dot / sqrMagnitude; + return r; + } + } + + /// @brief Project the vector on a plane defined by a normal orthogonal to the + /// plane. + /// @param v The vector to project + /// @param n The normal of the plane to project on + /// @return Teh projected vector + public static Vector3Float ProjectOnPlane(Vector3Float v, Vector3Float n) { + Vector3Float r = v - Project(v, n); + return r; + } + + /// @brief The angle between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The angle between the two vectors + /// @remark This reterns an unsigned angle which is the shortest distance + /// between the two vectors. Use Vector3::SignedAngle if a signed angle is + /// needed. + public static AngleFloat UnsignedAngle(Vector3Float v1, Vector3Float v2) { + float denominator = MathF.Sqrt(v1.sqrMagnitude * v2.sqrMagnitude); + if (denominator < Float.epsilon) + return AngleFloat.zero; + + float dot = Dot(v1, v2); + float fraction = dot / denominator; + if (float.IsNaN(fraction)) + return AngleFloat.Degrees( + fraction); // short cut to returning NaN universally + + float cdot = Float.Clamp(fraction, -1.0f, 1.0f); + float r = MathF.Acos(cdot); + return AngleFloat.Radians(r); + } + /// @brief The signed angle between two vectors + /// @param v1 The starting vector + /// @param v2 The ending vector + /// @param axis The axis to rotate around + /// @return The signed angle between the two vectors + public static AngleFloat SignedAngle(Vector3Float v1, Vector3Float v2, + Vector3Float axis) { + // angle in [0,180] + AngleFloat angle = UnsignedAngle(v1, v2); + + Vector3Float cross = Cross(v1, v2); + float b = Dot(axis, cross); + float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F); + + // angle in [-179,180] + AngleFloat signed_angle = angle * signd; + + return signed_angle; + } + + + /// @brief Lerp (linear interpolation) between two vectors + /// @param v1 The starting vector + /// @param v2 The ending vector + /// @param f The interpolation distance + /// @return The lerped vector + /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value + /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference + /// between *v1* and *v2* etc. + public static Vector3Float Lerp(Vector3Float v1, Vector3Float v2, float f) { + Vector3Float v = v1 + (v2 - v1) * f; + return v; + } + + } +} +//#endif \ No newline at end of file diff --git a/LinearAlgebra/src/Vector3Int.cs b/LinearAlgebra/src/Vector3Int.cs new file mode 100644 index 0000000..18edf40 --- /dev/null +++ b/LinearAlgebra/src/Vector3Int.cs @@ -0,0 +1,273 @@ +//#if !UNITY_5_3_OR_NEWER +using System; + +namespace LinearAlgebra { + + /// + /// 3-dimensional vectors + /// + /// This uses the right-handed coordinate system. + /// + /// Create a new 3-dimensional vector + /// + /// x axis value + /// y axis value + /// z axis value + public struct Vector3Int { + + /// + /// The right axis of the vector + /// + public int horizontal; //> left/right + /// + /// The upward axis of the vector + /// + public int vertical; //> up/down + /// + /// The forward axis of the vector + /// + public int depth; //> forward/backward + + public Vector3Int(int horizontal, int vertical, int depth) { + this.horizontal = horizontal; + this.vertical = vertical; + this.depth = depth; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3Int zero = new(0, 0, 0); + /// + /// A vector with one for all axis + /// + public static readonly Vector3Int one = new(1, 1, 1); + /// + /// A Vector3Int with values (-1, 0, 0) + /// + public static readonly Vector3Int left = new(-1, 0, 0); + /// + /// A vector with values (1, 0, 0) + /// + public static readonly Vector3Int right = new(1, 0, 0); + /// + /// A vector with values (0, -1, 0) + /// + public static readonly Vector3Int down = new(0, -1, 0); + /// + /// A vector with values (0, 1, 0) + /// + public static readonly Vector3Int up = new(0, 1, 0); + /// + /// A vector with values (0, 0, -1) + /// + public static readonly Vector3Int back = new(0, -1, 0); + /// + /// A vector with values (0, 0, 1) + /// + public static readonly Vector3Int forward = new(0, 1, 0); + + /// @brief The vector length + /// @return The vector length + public readonly float magnitude => MathF.Sqrt(horizontal * horizontal + vertical * vertical + depth * depth); + /// + /// The vector length + /// + /// The vector for which you need the length + /// The vector length + public static float MagnitudeOf(Vector3Int v) { + return v.magnitude; + } + + /// @brief The squared vector length + /// @return The squared vector length + /// @remark The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the + /// calculation of the squared root of C. + public readonly float sqrMagnitude => (horizontal * horizontal + vertical * vertical + depth * depth); + + /// + /// The squared vector length + /// + /// The vector for which you need the squared length + /// The squared vector length + /// The squared length is computationally simpler than the real + /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the + /// calculation of the squared root of C. + public static float SqrMagnitudeOf(Vector3Int v) { + return v.sqrMagnitude; + } + + /// @brief Convert the vector to a length of 1 + /// @return The vector normalized to a length of 1 + public readonly Vector3Float normalized { + get { + float l = magnitude; + Vector3Float v = Vector3Float.zero; + if (l > Float.epsilon) + v = new Vector3Float(this) / l; + return v; + } + } + /// @brief Convert the vector to a length of 1 + /// @param v The vector to convert + /// @return The vector normalized to a length of 1 + public static Vector3Float Normalize(Vector3Int v) { + float num = v.magnitude; + Vector3Float result = Vector3Float.zero; + if (num > Float.epsilon) + result = new Vector3Float(v) / num; + + return result; + } + + /// + /// Negate te vector such that it points in the opposite direction + /// + /// + /// The negated vector + public static Vector3Int operator -(Vector3Int v1) { + Vector3Int v = new(-v1.horizontal, -v1.vertical, -v1.depth); + return v; + } + + /// + /// Subtract two vectors + /// + /// + /// + /// The result of the subtraction + public static Vector3Int operator -(Vector3Int v1, Vector3Int v2) { + Vector3Int v = new(v1.horizontal - v2.horizontal, v1.vertical - v2.vertical, v1.depth - v2.depth); + return v; + } + + /// + /// Add two vectors + /// + /// + /// + /// The result of the addition + public static Vector3Int operator +(Vector3Int v1, Vector3Int v2) { + Vector3Int v = new(v1.horizontal + v2.horizontal, v1.vertical + v2.vertical, v1.depth + v2.depth); + return v; + } + + /// @brief Scale the vector using another vector + /// @param v1 The vector to scale + /// @param v2 A vector with the scaling factors + /// @return The scaled vector + /// @remark Each component of the vector v1 will be multiplied with the + /// matching component from the scaling vector v2. + public static Vector3Int Scale(Vector3Int v1, Vector3Int v2) { + return new Vector3Int(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical, v1.depth * v2.depth); + } + + + public static Vector3Int operator *(Vector3Int v1, int d) { + Vector3Int v = new(v1.horizontal * d, v1.vertical * d, v1.depth * d); + return v; + } + + public static Vector3Int operator *(int d, Vector3Int v1) { + Vector3Int v = new(d * v1.horizontal, d * v1.vertical, d * v1.depth); + return v; + } + + public static Vector3Int operator /(Vector3Int v1, int d) { + Vector3Int v = new(v1.horizontal / d, v1.vertical / d, v1.depth / d); + return v; + } + + public bool Equals(Vector3Int v) => (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); + + public override bool Equals(object obj) { + if (!(obj is Vector3Int v)) + return false; + + return (horizontal == v.horizontal && vertical == v.vertical && depth == v.depth); + } + + public static bool operator ==(Vector3Int v1, Vector3Int v2) { + return (v1.horizontal == v2.horizontal && v1.vertical == v2.vertical && v1.depth == v2.depth); + } + + public static bool operator !=(Vector3Int v1, Vector3Int v2) { + return (v1.horizontal != v2.horizontal || v1.vertical != v2.vertical || v1.depth != v2.depth); + } + + public override int GetHashCode() { + return (horizontal, vertical, depth).GetHashCode(); + } + + /// @brief The distance between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The distance between the two vectors + public static float Distance(Vector3Int v1, Vector3Int v2) { + return (v2 - v1).magnitude; + } + + /// @brief The dot product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The dot product of the two vectors + public static float Dot(Vector3Int v1, Vector3Int v2) { + return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical + v1.depth * v2.depth; + } + + /// @brief The cross product of two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The cross product of the two vectors + public static Vector3Int Cross(Vector3Int v1, Vector3Int v2) { + return new Vector3Int(v1.vertical * v2.depth - v1.depth * v2.vertical, v1.depth * v2.horizontal - v1.horizontal * v2.depth, + v1.horizontal * v2.vertical - v1.vertical * v2.horizontal); + + } + + /// @brief The angle between two vectors + /// @param v1 The first vector + /// @param v2 The second vector + /// @return The angle between the two vectors + /// @remark This reterns an unsigned angle which is the shortest distance + /// between the two vectors. Use Vector3::SignedAngle if a signed angle is + /// needed. + public static AngleFloat UnsignedAngle(Vector3Int v1, Vector3Int v2) { + float denominator = MathF.Sqrt(v1.sqrMagnitude * v2.sqrMagnitude); + if (denominator < Float.epsilon) + return AngleFloat.zero; + + float dot = Dot(v1, v2); + float fraction = dot / denominator; + if (float.IsNaN(fraction)) + return AngleFloat.Degrees( + fraction); // short cut to returning NaN universally + + float cdot = Float.Clamp(fraction, -1.0f, 1.0f); + float r = MathF.Acos(cdot); + return AngleFloat.Radians(r); + } + /// @brief The signed angle between two vectors + /// @param v1 The starting vector + /// @param v2 The ending vector + /// @param axis The axis to rotate around + /// @return The signed angle between the two vectors + public static AngleFloat SignedAngle(Vector3Int v1, Vector3Int v2, + Vector3Int axis) { + // angle in [0,180] + AngleFloat angle = UnsignedAngle(v1, v2); + + Vector3Int cross = Cross(v1, v2); + float b = Dot(axis, cross); + float signd = b < 0 ? -1.0F : (b > 0 ? 1.0F : 0.0F); + + // angle in [-179,180] + AngleFloat signed_angle = angle * signd; + + return signed_angle; + } + + } +} +//#endif \ No newline at end of file diff --git a/LinearAlgebra/src/float16.cs b/LinearAlgebra/src/float16.cs new file mode 100644 index 0000000..4b58cdd --- /dev/null +++ b/LinearAlgebra/src/float16.cs @@ -0,0 +1,322 @@ +using System; + +namespace LinearAlgebra { + + public class float16 { + // + // FILE: float16.cpp + // AUTHOR: Rob Tillaart + // VERSION: 0.1.8 + // PURPOSE: library for Float16s for Arduino + // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + + ushort _value; + + public float16() { _value = 0; } + + public float16(float f) { + //_value = f32tof16(f); + _value = F32ToF16__(f); + } + + public float toFloat() { + return f16tof32(_value); + } + + public ushort GetBinary() { return _value; } + public void SetBinary(ushort value) { _value = value; } + + ////////////////////////////////////////////////////////// + // + // EQUALITIES + // + /* + bool float16::operator ==(const float16 &f) { return (_value == f._value); } + + bool float16::operator !=(const float16 &f) { return (_value != f._value); } + + bool float16::operator >(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; + } + + bool float16::operator >=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; + } + + bool float16::operator <(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; + } + + bool float16::operator <=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; + } + + ////////////////////////////////////////////////////////// + // + // NEGATION + // + float16 float16::operator -() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; + } + + ////////////////////////////////////////////////////////// + // + // MATH + // + float16 float16::operator +(const float16 &f) { + return float16(this->toDouble() + f.toDouble()); + } + + float16 float16::operator -(const float16 &f) { + return float16(this->toDouble() - f.toDouble()); + } + + float16 float16::operator *(const float16 &f) { + return float16(this->toDouble() * f.toDouble()); + } + + float16 float16::operator /(const float16 &f) { + return float16(this->toDouble() / f.toDouble()); + } + + float16 & float16::operator+=(const float16 &f) { + *this = this->toDouble() + f.toDouble(); + return *this; + } + + float16 & float16::operator-=(const float16 &f) { + *this = this->toDouble() - f.toDouble(); + return *this; + } + + float16 & float16::operator*=(const float16 &f) { + *this = this->toDouble() * f.toDouble(); + return *this; + } + + float16 & float16::operator/=(const float16 &f) { + *this = this->toDouble() / f.toDouble(); + return *this; + } + + ////////////////////////////////////////////////////////// + // + // MATH HELPER FUNCTIONS + // + int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; + } + + bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + + bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; + } + + bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + */ + ////////////////////////////////////////////////////////// + // + // CORE CONVERSION + // + float f16tof32(ushort _value) { + //ushort sgn; + ushort man; + int exp; + float f; + + //Debug.Log($"{_value}"); + + bool sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (ushort)(_value & 0x03FF); + + //Debug.Log($"{sgn} {exp} {man}"); + + // ZERO + if ((_value & 0x7FFF) == 0) { + return sgn ? -0 : 0; + } + // NAN & INF + if (exp == 0x001F) { + if (man == 0) + return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; + else + return float.NaN; // NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + for (int i = 9; i >= 0; i--) { + f *= 2; + if ((man & (1 << i)) != 0) + f = f + 1; + } + //Debug.Log($"{f}"); + f = f * (float)Math.Pow(2.0f, exp - 25); + if (exp == 0) { + f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; + } + //Debug.Log($"{f}"); + return sgn ? -f : f; + } + + public static uint SingleToInt32Bits(float value) { + byte[] bytes = BitConverter.GetBytes(value); + if (BitConverter.IsLittleEndian) + Array.Reverse(bytes); // If the system is little-endian, reverse the byte order + return BitConverter.ToUInt32(bytes, 0); + } + + public ushort F32ToF16__(float f) { + uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0); + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + int exp = (int)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | exp | man); + return (ushort)(exp | man); + } + + //This function is faulty!!!! + ushort f32tof16(float f) { + //uint t = *(uint*)&f; + //uint t = (uint)BitConverter.SingleToInt32Bits(f); + uint t = SingleToInt32Bits(f); + // man bits = 10; but we keep 11 for rounding + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + short exp = (short)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = (short)(exp - 127 + 15); + // overflow does not fit => INF + if (exp > 30) { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + ushort uexp = (ushort)exp; + if (sgn) + return (ushort)(0x8000 | uexp | man); + return (ushort)(uexp | man); + } + + // -- END OF FILE -- + } + +} \ No newline at end of file diff --git a/LinearAlgebra/test/AngleTest.cs b/LinearAlgebra/test/AngleTest.cs new file mode 100644 index 0000000..8362d82 --- /dev/null +++ b/LinearAlgebra/test/AngleTest.cs @@ -0,0 +1,501 @@ +#if !UNITY_5_6_OR_NEWER +using System; +using System.Formats.Asn1; +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class AngleTests { + [SetUp] + public void Setup() { + } + + [Test] + public void Construct() { + // Degrees + float angle = 0.0f; + AngleFloat a = AngleFloat.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); + + angle = -180.0f; + a = AngleFloat.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); + + angle = 270.0f; + a = AngleFloat.Degrees(angle); + Assert.AreEqual(-90, a.inDegrees); + + angle = -270.0f; + a = AngleFloat.Degrees(angle); + Assert.AreEqual(90, a.inDegrees); + + // Radians + angle = 0.0f; + a = AngleFloat.Radians(angle); + Assert.AreEqual(angle, a.inRadians); + + angle = (float)-Math.PI; + a = AngleFloat.Radians(angle); + Assert.AreEqual(angle, a.inRadians); + + angle = (float)Math.PI * 1.5f; + a = AngleFloat.Radians(angle); + Assert.AreEqual(-Math.PI * 0.5f, a.inRadians, 1.0E-05F); + + // Revolutions + angle = 0.0f; + a = AngleFloat.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); + + angle = -0.5f; + a = AngleFloat.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); + + angle = 0.75f; + a = AngleFloat.Revolutions(angle); + Assert.AreEqual(-0.25f, a.inRevolutions); + + } + + [Test] + public void Revolutions() { + AngleFloat a; + + // Test zero + a = AngleFloat.Revolutions(0.0f); + Assert.AreEqual(0.0f, a.inRevolutions); + + // Test positive values within range + a = AngleFloat.Revolutions(0.25f); + Assert.AreEqual(0.25f, a.inRevolutions); + + a = AngleFloat.Revolutions(0.5f); + Assert.AreEqual(-0.5f, a.inRevolutions); + + // Test negative values within range + a = AngleFloat.Revolutions(-0.25f); + Assert.AreEqual(-0.25f, a.inRevolutions); + + a = AngleFloat.Revolutions(-0.5f); + Assert.AreEqual(-0.5f, a.inRevolutions); + + // Test values outside range (positive) + a = AngleFloat.Revolutions(1.0f); + Assert.AreEqual(0.0f, a.inRevolutions); + + a = AngleFloat.Revolutions(1.25f); + Assert.AreEqual(0.25f, a.inRevolutions); + + a = AngleFloat.Revolutions(1.75f); + Assert.AreEqual(-0.25f, a.inRevolutions); + + // Test values outside range (negative) + a = AngleFloat.Revolutions(-1.0f); + Assert.AreEqual(0.0f, a.inRevolutions); + + a = AngleFloat.Revolutions(-1.25f); + Assert.AreEqual(-0.25f, a.inRevolutions); + + a = AngleFloat.Revolutions(-1.75f); + Assert.AreEqual(0.25f, a.inRevolutions); + + // Test infinity + a = AngleFloat.Revolutions(float.PositiveInfinity); + Assert.AreEqual(float.PositiveInfinity, a.inRevolutions); + + a = AngleFloat.Revolutions(float.NegativeInfinity); + Assert.AreEqual(float.NegativeInfinity, a.inRevolutions); + } + + [Test] + public void Equality() { + // Test equality operator + Assert.IsTrue(AngleFloat.Degrees(90) == AngleFloat.Degrees(90), "90 == 90"); + Assert.IsFalse(AngleFloat.Degrees(90) == AngleFloat.Degrees(45), "90 == 45"); + Assert.IsTrue(AngleFloat.Degrees(0) == AngleFloat.Degrees(0), "0 == 0"); + Assert.IsTrue(AngleFloat.Degrees(-180) == AngleFloat.Degrees(-180), "-180 == -180"); + + // Test inequality operator + Assert.IsTrue(AngleFloat.Degrees(90) != AngleFloat.Degrees(45), "90 != 45"); + Assert.IsFalse(AngleFloat.Degrees(90) != AngleFloat.Degrees(90), "90 != 90"); + Assert.IsTrue(AngleFloat.Degrees(0) != AngleFloat.Degrees(1), "0 != 1"); + + // Test greater than operator + Assert.IsTrue(AngleFloat.Degrees(90) > AngleFloat.Degrees(45), "90 > 45"); + Assert.IsFalse(AngleFloat.Degrees(45) > AngleFloat.Degrees(90), "45 > 90"); + Assert.IsFalse(AngleFloat.Degrees(90) > AngleFloat.Degrees(90), "90 > 90"); + + // Test greater than or equal operator + Assert.IsTrue(AngleFloat.Degrees(90) >= AngleFloat.Degrees(45), "90 >= 45"); + Assert.IsTrue(AngleFloat.Degrees(90) >= AngleFloat.Degrees(90), "90 >= 90"); + Assert.IsFalse(AngleFloat.Degrees(45) >= AngleFloat.Degrees(90), "45 >= 90"); + + // Test less than operator + Assert.IsTrue(AngleFloat.Degrees(45) < AngleFloat.Degrees(90), "45 < 90"); + Assert.IsFalse(AngleFloat.Degrees(90) < AngleFloat.Degrees(45), "90 < 45"); + Assert.IsFalse(AngleFloat.Degrees(90) < AngleFloat.Degrees(90), "90 < 90"); + + // Test less than or equal operator + Assert.IsTrue(AngleFloat.Degrees(45) <= AngleFloat.Degrees(90), "45 <= 90"); + Assert.IsTrue(AngleFloat.Degrees(90) <= AngleFloat.Degrees(90), "90 <= 90"); + Assert.IsFalse(AngleFloat.Degrees(90) <= AngleFloat.Degrees(45), "90 <= 45"); + } + + // [Test] + // public void Normalize() { + // float r = 0; + + // r = Angle.Normalize(90); + // Assert.AreEqual(r, 90, "Normalize 90"); + + // r = Angle.Normalize(-90); + // Assert.AreEqual(r, -90, "Normalize -90"); + + // r = Angle.Normalize(270); + // Assert.AreEqual(r, -90, "Normalize 270"); + + // r = Angle.Normalize(270 + 360); + // Assert.AreEqual(r, -90, "Normalize 270+360"); + + // r = Angle.Normalize(-270); + // Assert.AreEqual(r, 90, "Normalize -270"); + + // r = Angle.Normalize(-270 - 360); + // Assert.AreEqual(r, 90, "Normalize -270-360"); + + // r = Angle.Normalize(0); + // Assert.AreEqual(r, 0, "Normalize 0"); + + // r = Angle.Normalize(float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + // r = Angle.Normalize(float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + // } + + [Test] + public void Clamp() { + float r = 0; + + r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(0), AngleFloat.Degrees(2)); + Assert.AreEqual(1, r, "Clamp 1 0 2"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(-1), AngleFloat.Degrees(0), AngleFloat.Degrees(2)); + Assert.AreEqual(0, r, "Clamp -1 0 2"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(3), AngleFloat.Degrees(0), AngleFloat.Degrees(2)); + Assert.AreEqual(2, r, "Clamp 3 0 2"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(0), AngleFloat.Degrees(0)); + Assert.AreEqual(0, r, "Clamp 1 0 0"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(0), AngleFloat.Degrees(0), AngleFloat.Degrees(0)); + Assert.AreEqual(0, r, "Clamp 0 0 0"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(0), AngleFloat.Degrees(1), AngleFloat.Degrees(-1)); + Assert.AreEqual(1, r, "Clamp 0 1 -1"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(0), AngleFloat.Degrees(float.PositiveInfinity)); + Assert.AreEqual(1, r, "Clamp 1 0 INFINITY"); + + r = AngleFloat.Clamp(AngleFloat.Degrees(1), AngleFloat.Degrees(float.NegativeInfinity), AngleFloat.Degrees(1)); + Assert.AreEqual(1, r, "Clamp 1 -INFINITY 1"); + } + + [Test] + public void Cos() { + // Test zero + Assert.AreEqual(1.0f, AngleFloat.Cos(AngleFloat.Degrees(0)), 1.0E-05F, "Cos(0°)"); + + // Test 90 degrees + Assert.AreEqual(0.0f, AngleFloat.Cos(AngleFloat.Degrees(90)), 1.0E-05F, "Cos(90°)"); + + // Test 180 degrees + Assert.AreEqual(-1.0f, AngleFloat.Cos(AngleFloat.Degrees(180)), 1.0E-05F, "Cos(180°)"); + + // Test 270 degrees + Assert.AreEqual(0.0f, AngleFloat.Cos(AngleFloat.Degrees(270)), 1.0E-05F, "Cos(270°)"); + + // Test 45 degrees + Assert.AreEqual(MathF.Sqrt(2) / 2, AngleFloat.Cos(AngleFloat.Degrees(45)), 1.0E-05F, "Cos(45°)"); + + // Test negative angle + Assert.AreEqual(1.0f, AngleFloat.Cos(AngleFloat.Degrees(-360)), 1.0E-05F, "Cos(-360°)"); + + // Test using radians + Assert.AreEqual(1.0f, AngleFloat.Cos(AngleFloat.Radians(0)), 1.0E-05F, "Cos(0 rad)"); + Assert.AreEqual(0.0f, AngleFloat.Cos(AngleFloat.Radians((float)Math.PI / 2)), 1.0E-05F, "Cos(π/2)"); + Assert.AreEqual(-1.0f, AngleFloat.Cos(AngleFloat.Radians((float)Math.PI)), 1.0E-05F, "Cos(π)"); + } + + [Test] + public void Sin() { + // Test zero + Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Degrees(0)), 1.0E-05F, "Sin(0°)"); + + // Test 90 degrees + Assert.AreEqual(1.0f, AngleFloat.Sin(AngleFloat.Degrees(90)), 1.0E-05F, "Sin(90°)"); + + // Test 180 degrees + Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Degrees(180)), 1.0E-05F, "Sin(180°)"); + + // Test 270 degrees + Assert.AreEqual(-1.0f, AngleFloat.Sin(AngleFloat.Degrees(270)), 1.0E-05F, "Sin(270°)"); + + // Test 45 degrees + Assert.AreEqual(MathF.Sqrt(2) / 2, AngleFloat.Sin(AngleFloat.Degrees(45)), 1.0E-05F, "Sin(45°)"); + + // Test negative angle + Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Degrees(-360)), 1.0E-05F, "Sin(-360°)"); + + // Test using radians + Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Radians(0)), 1.0E-05F, "Sin(0 rad)"); + Assert.AreEqual(1.0f, AngleFloat.Sin(AngleFloat.Radians((float)Math.PI / 2)), 1.0E-05F, "Sin(π/2)"); + Assert.AreEqual(0.0f, AngleFloat.Sin(AngleFloat.Radians((float)Math.PI)), 1.0E-05F, "Sin(π)"); + } + + [Test] + public void Tan() { + // Test zero + Assert.AreEqual(0.0f, AngleFloat.Tan(AngleFloat.Degrees(0)), 1.0E-05F, "Tan(0°)"); + + // Test 45 degrees + Assert.AreEqual(1.0f, AngleFloat.Tan(AngleFloat.Degrees(45)), 1.0E-05F, "Tan(45°)"); + + // Test -45 degrees + Assert.AreEqual(-1.0f, AngleFloat.Tan(AngleFloat.Degrees(-45)), 1.0E-05F, "Tan(-45°)"); + + // Test using radians + Assert.AreEqual(0.0f, AngleFloat.Tan(AngleFloat.Radians(0)), 1.0E-05F, "Tan(0 rad)"); + Assert.AreEqual(1.0f, AngleFloat.Tan(AngleFloat.Radians((float)Math.PI / 4)), 1.0E-05F, "Tan(π/4)"); + } + + [Test] + public void Acos() { + // Test 1 (0 degrees) + Assert.AreEqual(0.0f, AngleFloat.Acos(1.0f).inRadians, 1.0E-05F, "Acos(1)"); + + // Test 0 (90 degrees or π/2 radians) + Assert.AreEqual((float)Math.PI / 2, AngleFloat.Acos(0.0f).inRadians, 1.0E-05F, "Acos(0)"); + + // Test -1 (-180 degrees or π radians) + Assert.AreEqual((float)-Math.PI, AngleFloat.Acos(-1.0f).inRadians, 1.0E-05F, "Acos(-1)"); + + // Test 0.5 (60 degrees or π/3 radians) + Assert.AreEqual((float)Math.PI / 3, AngleFloat.Acos(0.5f).inRadians, 1.0E-05F, "Acos(0.5)"); + + // Test sqrt(2)/2 (45 degrees or π/4 radians) + Assert.AreEqual((float)Math.PI / 4, AngleFloat.Acos(MathF.Sqrt(2) / 2).inRadians, 1.0E-05F, "Acos(√2/2)"); + } + + [Test] + public void Asin() { + // Test 0 (0 degrees) + Assert.AreEqual(0.0f, AngleFloat.Asin(0.0f).inRadians, 1.0E-05F, "Asin(0)"); + + // Test 1 (90 degrees or π/2 radians) + Assert.AreEqual((float)Math.PI / 2, AngleFloat.Asin(1.0f).inRadians, 1.0E-05F, "Asin(1)"); + + // Test -1 (-90 degrees or -π/2 radians) + Assert.AreEqual(-(float)Math.PI / 2, AngleFloat.Asin(-1.0f).inRadians, 1.0E-05F, "Asin(-1)"); + + // Test 0.5 (30 degrees or π/6 radians) + Assert.AreEqual((float)Math.PI / 6, AngleFloat.Asin(0.5f).inRadians, 1.0E-05F, "Asin(0.5)"); + + // Test sqrt(2)/2 (45 degrees or π/4 radians) + Assert.AreEqual((float)Math.PI / 4, AngleFloat.Asin(MathF.Sqrt(2) / 2).inRadians, 1.0E-05F, "Asin(√2/2)"); + } + + + [Test] + public void Atan() { + // Test zero + Assert.AreEqual(0.0f, AngleFloat.Atan(0.0f).inRadians, 1.0E-05F, "Atan(0)"); + + // Test 1 (45 degrees or π/4 radians) + Assert.AreEqual((float)Math.PI / 4, AngleFloat.Atan(1.0f).inRadians, 1.0E-05F, "Atan(1)"); + + // Test -1 (-45 degrees or -π/4 radians) + Assert.AreEqual(-(float)Math.PI / 4, AngleFloat.Atan(-1.0f).inRadians, 1.0E-05F, "Atan(-1)"); + + // Test sqrt(3) (60 degrees or π/3 radians) + Assert.AreEqual((float)Math.PI / 3, AngleFloat.Atan(MathF.Sqrt(3)).inRadians, 1.0E-05F, "Atan(√3)"); + + // Test 1/sqrt(3) (30 degrees or π/6 radians) + Assert.AreEqual((float)Math.PI / 6, AngleFloat.Atan(1.0f / MathF.Sqrt(3)).inRadians, 1.0E-05F, "Atan(1/√3)"); + + // Test positive infinity + Assert.AreEqual((float)Math.PI / 2, AngleFloat.Atan(float.PositiveInfinity).inRadians, 1.0E-05F, "Atan(+∞)"); + + // Test negative infinity + Assert.AreEqual(-(float)Math.PI / 2, AngleFloat.Atan(float.NegativeInfinity).inRadians, 1.0E-05F, "Atan(-∞)"); + } + + [Test] + public void Atan2() { + // Test basic quadrant I + Assert.AreEqual((float)Math.PI / 4, AngleFloat.Atan2(1.0f, 1.0f).inRadians, 1.0E-05F, "Atan2(1, 1)"); + + // Test quadrant II + Assert.AreEqual(3 * (float)Math.PI / 4, AngleFloat.Atan2(1.0f, -1.0f).inRadians, 1.0E-05F, "Atan2(1, -1)"); + + // Test quadrant III + Assert.AreEqual(-(float)Math.PI * 0.75f, AngleFloat.Atan2(-1.0f, -1.0f).inRadians, 1.0E-05F, "Atan2(-1, -1)"); + + // Test quadrant IV + Assert.AreEqual(-(float)Math.PI / 4, AngleFloat.Atan2(-1.0f, 1.0f).inRadians, 1.0E-05F, "Atan2(-1, 1)"); + + // Test positive x-axis + Assert.AreEqual(0.0f, AngleFloat.Atan2(0.0f, 1.0f).inRadians, 1.0E-05F, "Atan2(0, 1)"); + + // Test positive y-axis + Assert.AreEqual((float)Math.PI / 2, AngleFloat.Atan2(1.0f, 0.0f).inRadians, 1.0E-05F, "Atan2(1, 0)"); + + // Test negative y-axis + Assert.AreEqual(-(float)Math.PI / 2, AngleFloat.Atan2(-1.0f, 0.0f).inRadians, 1.0E-05F, "Atan2(-1, 0)"); + + // Test origin + Assert.AreEqual(0.0f, AngleFloat.Atan2(0.0f, 0.0f).inRadians, 1.0E-05F, "Atan2(0, 0)"); + + // Test with different magnitudes + Assert.AreEqual((float)Math.PI / 3, AngleFloat.Atan2(MathF.Sqrt(3), 1.0f).inRadians, 1.0E-05F, "Atan2(√3, 1)"); + + // Test negative x-axis + Assert.AreEqual((float)-Math.PI, AngleFloat.Atan2(0.0f, -1.0f).inRadians, 1.0E-05F, "Atan2(0, -1)"); + } + + [Test] + public void Multiplication() { + AngleFloat r = AngleFloat.zero; + + // Angle * float + r = AngleFloat.Degrees(90) * 2; + Assert.AreEqual(-180, r.inDegrees, "Multiply 90 * 2"); + + r = AngleFloat.Degrees(45) * 0.5f; + Assert.AreEqual(22.5f, r.inDegrees, "Multiply 45 * 0.5"); + + r = AngleFloat.Degrees(90) * 0; + Assert.AreEqual(0, r.inDegrees, "Multiply 90 * 0"); + + r = AngleFloat.Degrees(-90) * 2; + Assert.AreEqual(-180, r.inDegrees, "Multiply -90 * 2"); + + r = AngleFloat.Degrees(270) * 2; + Assert.AreEqual(-180, r.inDegrees, "Multiply 270 * 2 (normalized)"); + + // float * Angle + r = 2 * AngleFloat.Degrees(90); + Assert.AreEqual(-180, r.inDegrees, "Multiply 2 * 90"); + + r = 0.5f * AngleFloat.Degrees(45); + Assert.AreEqual(22.5, r.inDegrees, "Multiply 0.5 * 45"); + + r = 0 * AngleFloat.Degrees(90); + Assert.AreEqual(0, r.inDegrees, "Multiply 0 * 90"); + + r = 2 * AngleFloat.Degrees(-90); + Assert.AreEqual(-180, r.inDegrees, "Multiply 2 * -90"); + + // Negative factor + r = AngleFloat.Degrees(90) * -1; + Assert.AreEqual(-90, r.inDegrees, "Multiply 90 * -1"); + + r = -1 * AngleFloat.Degrees(90); + Assert.AreEqual(-90, r.inDegrees, "Multiply -1 * 90"); + } + + [Test] + public void MoveTowards() { + AngleFloat r = AngleFloat.zero; + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 30); + Assert.AreEqual(30, r.inDegrees, "MoveTowards 0 90 30"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 90); + Assert.AreEqual(90, r.inDegrees, "MoveTowards 0 90 90"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 180); + Assert.AreEqual(90, r.inDegrees, "MoveTowards 0 90 180"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 270); + Assert.AreEqual(90, r.inDegrees, "MoveTowrads 0 90 270"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), -30); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 90 -30"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -30); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -30"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -90); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -90"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -180); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -180"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), -270); + Assert.AreEqual(0, r.inDegrees, "MoveTowrads 0 -90 -270"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), 0); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 90 0"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(0), 0); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 0 0"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(0), 30); + Assert.AreEqual(0, r.inDegrees, "MoveTowrads 0 0 30"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(90), float.PositiveInfinity); + Assert.AreEqual(90, r.inDegrees, "MoveTowards 0 90 INFINITY"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(float.PositiveInfinity), 30); + Assert.AreEqual(30, r.inDegrees, "MoveTowrads 0 INFINITY 30"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(-90), float.NegativeInfinity); + Assert.AreEqual(0, r.inDegrees, "MoveTowards 0 -90 -INFINITY"); + + r = AngleFloat.MoveTowards(AngleFloat.Degrees(0), AngleFloat.Degrees(float.NegativeInfinity), -30); + Assert.AreEqual(0, r.inDegrees, "MoveTowrads 0 -INFINITY -30"); + + } + + [Test] + public void Difference() { + float r = 0; + + r = Angles.Difference(0, 90); + Assert.AreEqual(90, r, "Difference 0 90"); + + r = Angles.Difference(0, -90); + Assert.AreEqual(-90, r, "Difference 0 -90"); + + r = Angles.Difference(0, 270); + Assert.AreEqual(-90, r, "Difference 0 270"); + + r = Angles.Difference(0, -270); + Assert.AreEqual(90, r, "Difference 0 -270"); + + r = Angles.Difference(90, 0); + Assert.AreEqual(-90, r, "Difference 90 0"); + + r = Angles.Difference(-90, 0); + Assert.AreEqual(90, r, "Difference -90 0"); + + r = Angles.Difference(0, 0); + Assert.AreEqual(0, r, "Difference 0 0"); + + r = Angles.Difference(90, 90); + Assert.AreEqual(0, r, "Difference 90 90"); + + r = Angles.Difference(0, float.PositiveInfinity); + Assert.AreEqual(float.PositiveInfinity, r, "Difference 0 INFINITY"); + + r = Angles.Difference(0, float.NegativeInfinity); + Assert.AreEqual(float.NegativeInfinity, r, "Difference 0 -INFINITY"); + + r = Angles.Difference(float.NegativeInfinity, float.PositiveInfinity); + Assert.AreEqual(float.PositiveInfinity, r, "Difference -INFINITY INFINITY"); + } + } + +} +#endif diff --git a/LinearAlgebra/test/DirectionTest.cs b/LinearAlgebra/test/DirectionTest.cs new file mode 100644 index 0000000..0eb9882 --- /dev/null +++ b/LinearAlgebra/test/DirectionTest.cs @@ -0,0 +1,226 @@ +#if !UNITY_5_6_OR_NEWER +using System; +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class DirectionTest { + + [Test] + public void RadiansForward() { + Direction d = Direction.Radians(0, 0); + Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(0, d.vertical.inDegrees, 0.0001f); + } + + [Test] + public void RadiansUp() { + Direction d = Direction.Radians(0, (float)Math.PI / 2); + Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(90, d.vertical.inDegrees, 0.0001f); + } + + [Test] + public void RadiansDown() { + Direction d = Direction.Radians(0, -(float)Math.PI / 2); + Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(-90, d.vertical.inDegrees, 0.0001f); + } + + [Test] + public void RadiansArbitrary() { + Direction d = Direction.Radians((float)Math.PI / 4, (float)Math.PI / 6); + Assert.AreEqual(45, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(30, d.vertical.inDegrees, 0.0001f); + } + + [Test] + public void DegreesNormalize1() { + Direction d = Direction.Degrees(112, 91); + Assert.AreEqual(-68, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(89, d.vertical.inDegrees, 0.0001f); + } + + [Test] + public void RadiansEquivalentToDegreesConversion() { + Direction d1 = Direction.Radians((float)Math.PI / 3, (float)Math.PI / 4); + Direction d2 = Direction.Degrees(60, 45); + Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f); + } + + [Test] + public void ToVector3Forward() { + Direction d = Direction.forward; + Vector3Float v = d.ToVector3(); + Assert.AreEqual(0, v.horizontal, 0.0001f); + Assert.AreEqual(0, v.vertical, 0.0001f); + Assert.AreEqual(1, v.depth, 0.0001f); + } + + [Test] + public void ToVector3Up() { + Direction d = Direction.up; + Vector3Float v = d.ToVector3(); + Assert.AreEqual(0, v.horizontal, 0.0001f); + Assert.AreEqual(1, v.vertical, 0.0001f); + Assert.AreEqual(0, v.depth, 0.0001f); + } + + [Test] + public void ToVector3Down() { + Direction d = Direction.down; + Vector3Float v = d.ToVector3(); + Assert.AreEqual(0, v.horizontal, 0.0001f); + Assert.AreEqual(-1, v.vertical, 0.0001f); + Assert.AreEqual(0, v.depth, 0.0001f); + } + + [Test] + public void ToVector3Left() { + Direction d = Direction.left; + Vector3Float v = d.ToVector3(); + Assert.AreEqual(-1, v.horizontal, 0.0001f); + Assert.AreEqual(0, v.vertical, 0.0001f); + Assert.AreEqual(0, v.depth, 0.0001f); + } + + [Test] + public void FromVector3Forward() { + Vector3Float v = new(0, 0, 1); + Direction d = Direction.FromVector3(v); + Assert.AreEqual(0, d.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(0, d.vertical.inDegrees, 0.0001f); + } + + [Test] + public void ToVector3AndBack() { + Direction d1 = Direction.Degrees(45, 30); + Vector3Float v = d1.ToVector3(); + Direction d2 = Direction.FromVector3(v); + Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f); + } + + [Test] + public void ToVector3AndBack2() { + Direction d1 = Direction.Degrees(-135, 85); + Vector3Float v = d1.ToVector3(); + Direction d2 = Direction.FromVector3(v); + Assert.AreEqual(d1.horizontal.inDegrees, d2.horizontal.inDegrees, 0.0001f); + Assert.AreEqual(d1.vertical.inDegrees, d2.vertical.inDegrees, 0.0001f); + } + + [Test] + public void Compare() { + Direction d1 = Direction.Degrees(45, 135); + Direction d2 = new(AngleFloat.Degrees(45), AngleFloat.Degrees(135)); + bool r; + r = d1 == d2; + Assert.True(r); + Assert.AreEqual(d1, d2); + } + + [Test] + public void NotEqualWithDifferentHorizontal() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(90, 30); + Assert.True(d1 != d2); + } + + [Test] + public void NotEqualWithDifferentVertical() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(45, 60); + Assert.True(d1 != d2); + } + + [Test] + public void NotEqualWithDifferentBoth() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(90, 60); + Assert.True(d1 != d2); + } + + [Test] + public void NotEqualWithSameValues() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(45, 30); + Assert.False(d1 != d2); + } + + + [Test] + public void EqualsWithSameValues() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(45, 30); + Assert.True(d1.Equals(d2)); + } + + [Test] + public void EqualsWithDifferentHorizontal() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(90, 30); + Assert.False(d1.Equals(d2)); + } + + [Test] + public void EqualsWithDifferentVertical() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(45, 60); + Assert.False(d1.Equals(d2)); + } + + [Test] + public void EqualsWithDifferentBoth() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(90, 60); + Assert.False(d1.Equals(d2)); + } + + [Test] + public void EqualsWithNonDirectionObject() { + Direction d = Direction.Degrees(45, 30); + Assert.False(d.Equals("not a direction")); + } + + [Test] + public void EqualsWithNull() { + Direction d = Direction.Degrees(45, 30); + Assert.False(d.Equals(null)); + } + + [Test] + public void EqualsWithZeros() { + Direction d1 = Direction.forward; + Direction d2 = Direction.Degrees(0, 0); + Assert.True(d1.Equals(d2)); + } + + [Test] + public void HashCode() { + Direction d1 = Direction.Degrees(45, 30); + Direction d2 = Direction.Degrees(45, 30); + Assert.AreEqual(d1.GetHashCode(), d2.GetHashCode()); + + d1 = Direction.Degrees(45, 30); + d2 = Direction.Degrees(90, 30); + Assert.AreNotEqual(d1.GetHashCode(), d2.GetHashCode()); + + d1 = Direction.Degrees(45, 30); + d2 = Direction.Degrees(45, 60); + Assert.AreNotEqual(d1.GetHashCode(), d2.GetHashCode()); + + Direction d = Direction.Degrees(45, 30); + int hash1 = d.GetHashCode(); + int hash2 = d.GetHashCode(); + Assert.AreEqual(hash1, hash2); + + d1 = Direction.forward; + d2 = Direction.Degrees(0, 0); + Assert.AreEqual(d1.GetHashCode(), d2.GetHashCode()); + } + + }; +} +#endif + diff --git a/LinearAlgebra/test/LinearAlgebra_Test.csproj b/LinearAlgebra/test/LinearAlgebra_Test.csproj new file mode 100644 index 0000000..5b48e60 --- /dev/null +++ b/LinearAlgebra/test/LinearAlgebra_Test.csproj @@ -0,0 +1,19 @@ + + + + net8.0 + false + true + + + + + + + + + + + + + diff --git a/LinearAlgebra/test/QuaternionTest.cs b/LinearAlgebra/test/QuaternionTest.cs new file mode 100644 index 0000000..9dd5a96 --- /dev/null +++ b/LinearAlgebra/test/QuaternionTest.cs @@ -0,0 +1,185 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + + public class QuaternionTest { + + [SetUp] + public void Setup() { + } + + [Test] + public void Normalize() { + Quaternion q1 = new(0, 0, 0, 1); + Quaternion r = Quaternion.identity; + + r = q1.normalized; + Assert.AreEqual(r, q1, "q.normalized 0 0 0 1"); + + r = Quaternion.Normalize(q1); + Assert.AreEqual(r, q1, "q.normalized 0 0 0 1"); + } + + [Test] + public void ToAngles() { + Quaternion q1 = new(0, 0, 0, 1); + Vector3Float v = Vector3Float.zero; + + v = Quaternion.ToAngles(q1); + Assert.AreEqual(v, new Vector3Float(0, 0, 0), "ToAngles 0 0 0 1"); + + q1 = new(1, 0, 0, 0); + v = Quaternion.ToAngles(q1); + Assert.AreEqual(0, v.horizontal, "1 0 0 0 H"); + Assert.AreEqual(180, v.vertical, "1 0 0 0 V"); + Assert.AreEqual(180, v.depth, "1 0 0 0 D"); + + } + + [Test] + public void Multiplication() { + Quaternion q1 = new(0, 0, 0, 1); + Quaternion q2 = new(1, 0, 0, 0); + Quaternion r; + + r = q1 * q2; + Assert.AreEqual(r, new Quaternion(1, 0, 0, 0), "0 0 0 1 * 1 0 0 0 "); + } + + [Test] + public void MultiplicationVector() { + Quaternion q1 = new(0, 0, 0, 1); + Vector3Float v1 = new(0, 1, 0); + Vector3Float r; + + r = q1 * v1; + Assert.AreEqual(r, new Vector3Float(0, 1, 0), "0 0 0 1 * Vector 0 1 0"); + + q1 = new(1, 0, 0, 0); + r = q1 * v1; + Assert.AreEqual(r, new Vector3Float(0, -1, 0), "1 0 0 0 * Vector 0 1 0"); + } + + [Test] + public void Equality() { + Quaternion q1 = new(0, 0, 0, 1); + Quaternion q2 = new(1, 0, 0, 0); + Assert.AreNotEqual(q1, q2, "0 0 0 1 == 1 0 0 0"); + + q2 = new(0, 0, 0, 1); + Assert.AreEqual(q1, q2, "0 0 0 1 == 1 0 0 0"); + } + + [Test, Ignore("ToDo")] + public void Inverse() { } + + [Test, Ignore("ToDo")] + public void LookRotation() { } + + [Test, Ignore("ToDo")] + public void FromToRotation() { } + + [Test, Ignore("ToDo")] + public void RotateTowards() { } + + [Test, Ignore("ToDo")] + public void AngleAxis() { } + + [Test, Ignore("ToDo")] + public void Angle() { } + + [Test, Ignore("ToDo")] + public void Slerp() { } + + [Test, Ignore("ToDo")] + public void SlerpUnclamped() { } + + [Test] + public void Euler() { + Vector3Float v1 = new(0, 0, 0); + Quaternion q; + + q = Quaternion.Euler(v1); + Assert.AreEqual(q, Quaternion.identity, "Euler Vector 0 0 0"); + + q = Quaternion.Euler(0, 0, 0); + Assert.AreEqual(q, Quaternion.identity, "Euler 0 0 0"); + + v1 = new(90, 90, -90); + q = Quaternion.Euler(v1); + Assert.AreEqual(q, new Quaternion(0, 0.707106709F, -0.707106709F, 0), "Euler Vector 90 90 -90"); + + q = Quaternion.Euler(90, 90, -90); + Assert.AreEqual(q, new Quaternion(0, 0.707106709F, -0.707106709F, 0), "Euler 90 90 -90"); + } + + [Test] + public void EulerToAngles() { + Vector3Float v; + Quaternion q; + Quaternion r; + + //v = new(0, 0, 0); + q = Quaternion.Euler(0, 0 , 0); + v = Quaternion.ToAngles(q); + r = Quaternion.Euler(v); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "0 0 0"); + + q = Quaternion.Euler(-45, -30, -15); + v = Quaternion.ToAngles(q); + r = Quaternion.Euler(v); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "-45, -30, -15"); + + // Gimball lock + // q = Quaternion.Euler(90, 90, -90); + // v = Quaternion.ToAngles(q); + // r = Quaternion.Euler(v); + // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f, "0 0 0"); + + } + + [Test] + public void GetAngleAround() { + Vector3Float v1 = new(0, 1, 0); + Quaternion q1 = new(0, 0, 0, 1); + + float f = Quaternion.GetAngleAround(v1, q1); + Assert.AreEqual(f, 0, "GetAngleAround 0 1 0 , 0 0 0 1"); + + q1 = new(0, 0.707106709F, -0.707106709F, 0); + f = Quaternion.GetAngleAround(v1, q1); + Assert.AreEqual(f, 180, "GetAngleAround 0 1 0 , 0 0.7 -0.7 0"); + + v1 = new(0, 0, 0); + f = Quaternion.GetAngleAround(v1, q1); + Assert.IsTrue(float.IsNaN(f), "GetAngleAround 0 0 0 , 0 0.7 -0.7 0"); + } + + [Test] + public void GetRotationAround() { + Vector3Float v1 = new(0, 1, 0); + Quaternion q1 = new(0, 0, 0, 1); + + Quaternion q = Quaternion.GetRotationAround(v1, q1); + Assert.AreEqual(q, new Quaternion(0, 0, 0, 1), "GetRotationAround 0 1 0 , 0 0 0 1"); + + q1 = new(0, 0.707106709F, -0.707106709F, 0); + q = Quaternion.GetRotationAround(v1, q1); + Assert.AreEqual(q, new Quaternion(0, 1, 0, 0), "GetRotationAround 0 1 0 , 0 0.7 -0.7 0"); + + v1 = new(0, 0, 0); + q = Quaternion.GetRotationAround(v1, q1); + bool r = float.IsNaN(q.x) && float.IsNaN(q.y) && float.IsNaN(q.z) && float.IsNaN(q.w); + Assert.IsTrue(r, "GetRotationAround 0 0 0 , 0 0.7 -0.7 0"); + } + + [Test, Ignore("ToDo")] + public void GetSwingTwist() { } + + [Test, Ignore("ToDo")] + public void Dot() { } + + } +} +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/SphericalTest.cs b/LinearAlgebra/test/SphericalTest.cs new file mode 100644 index 0000000..b28b9d9 --- /dev/null +++ b/LinearAlgebra/test/SphericalTest.cs @@ -0,0 +1,271 @@ +//#if !UNITY_5_6_OR_NEWER +using System; +using System.Collections.Generic; +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class SphericalTest { + [SetUp] + public void Setup() { + } + + [Test] + public void FromVector3() { +#if UNITY_5_6_OR_NEWER + UnityEngine.Vector3 v = new(0, 0, 1); +#else + Vector3Float v = new(0, 0, 1); +#endif + Spherical s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1"); + Assert.AreEqual(0.0f, s.direction.horizontal.inDegrees, "s.hor 0 0 1"); + Assert.AreEqual(0.0f, s.direction.vertical.inDegrees, 1.0E-05F, "s.vert 0 0 1"); + + v = new(0, 1, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0"); + Assert.AreEqual(0.0f, s.direction.horizontal.inDegrees, "s.hor 0 1 0"); + Assert.AreEqual(90.0f, s.direction.vertical.inDegrees, "s.vert 0 1 0"); + + v = new(1, 0, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0"); + Assert.AreEqual(90.0f, s.direction.horizontal.inDegrees, "s.hor 1 0 0"); + Assert.AreEqual(0.0f, s.direction.vertical.inDegrees, 1.0E-05F, "s.vert 1 0 0"); + } + + [Test] + public void Addition() { + Spherical v1 = Spherical.Degrees(1, 45, 0); + Spherical v2 = Spherical.zero; + Spherical r = Spherical.zero; + + r = v1 + v2; + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + r = v1; + r += v2; + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + v2 = Spherical.Degrees(1, 0, 90); + r = v1 + v2; + Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.horizontal.inDegrees, 1e-5f, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.vertical.inDegrees, 1.0E-05F, "Addition(1 0 90)"); + } + + [Test] + public void Average2_IdenticalVectors() { + Direction dir = Direction.Radians(MathF.PI / 4f, MathF.PI / 6f); + Spherical v = new(2.5f, dir); + + Spherical avg = Spherical.Average(v, v); + + Assert.AreEqual(2.5f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal, avg.direction.horizontal); + Assert.AreEqual(dir.vertical, avg.direction.vertical); + } + + [Test] + public void Average2_OppositeUnitVectors() { + // Two opposite vectors: same distance, horizontal opposite (pi apart), same vertical + Spherical v1 = Spherical.Radians(1f, 0f, 0f); + Spherical v2 = Spherical.Radians(1f, MathF.PI, 0f); + Spherical avg = Spherical.Average(v1, v2); + + Assert.AreEqual(0f, avg.distance, 1e-4f); + // When distance is zero, angles may be undefined; allow any angle but ensure near-zero magnitude + } + + [Test] + public void Average2_WeightedByDistance() { + // Two vectors same direction but different distances -> weighted average distance + Direction dir = Direction.Radians(MathF.PI / 3f, MathF.PI / 4f); + Spherical a = new(1f, dir); + Spherical b = new(3f, dir); + Spherical avg = Spherical.Average(a, b); + + // average distance should be (1+3)/2 = 2 + Assert.AreEqual(2f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f); + Assert.AreEqual(dir.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f); + } + + [Test] + public void Average2_OppositeButNotExact_NotZero() { + // Nearly opposite but not exact; expect a valid averaged direction and averaged distance + Direction d1 = Direction.Radians(0f, 0f); + Direction d2 = Direction.Radians(MathF.PI - 1e-3f, 0.0f); // slight offset + Spherical v1 = new(2.0f, d1); + Spherical v2 = new(4.0f, d2); + + Spherical avg = Spherical.Average(v1, v2); + + // Distance is arithmetic mean + Assert.AreEqual(3.0f, avg.distance, 1e-5f); + + // Averaged azimuth should be near +pi/2 or -pi/2? we can check it's not NaN and unit-vector properties hold + float ux = MathF.Cos(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uy = MathF.Sin(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uz = MathF.Sin(avg.direction.vertical.inRadians); + float mag = MathF.Sqrt(ux * ux + uy * uy + uz * uz); + Assert.IsTrue(mag > 0.999f && mag < 1.001f); + + } + + [Test] + public void Average2_BasicAverageDirectionAndDistance() { + // Two different directions not cancelling: expect vector-average result + Direction d1 = Direction.Radians(MathF.PI / 6f, MathF.PI / 12f); // 30°, 15° + Direction d2 = Direction.Radians(MathF.PI / 3f, MathF.PI / 18f); // 60°, 10° + Spherical v1 = new(2.0f, d1); + Spherical v2 = new(4.0f, d2); + + Spherical avg = Spherical.Average(v1, v2); + + // Distance is arithmetic mean + Assert.AreEqual(3.0f, avg.distance, 1e-5f); + + // Check averaged unit-vector equals normalized sum of unit vectors computed here + float a1 = d1.horizontal.inRadians; + float a2 = d2.horizontal.inRadians; + float e1 = d1.vertical.inRadians; + float e2 = d2.vertical.inRadians; + + float cx = MathF.Cos(a1) + MathF.Cos(a2); + float cy = MathF.Sin(a1) + MathF.Sin(a2); + float z1 = MathF.Sin(e1); + float z2 = MathF.Sin(e2); + float cz = z1 + z2; + float mag = MathF.Sqrt(cx * cx + cy * cy + cz * cz); + Assert.IsTrue(mag > 1e-6f); + + float ux = cx / mag; + float uy = cy / mag; + float uz = cz / mag; + + // Reconstruct direction from avg result + float uxAvg = MathF.Cos(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uyAvg = MathF.Sin(avg.direction.horizontal.inRadians) * MathF.Cos(avg.direction.vertical.inRadians); + float uzAvg = MathF.Sin(avg.direction.vertical.inRadians); + + Assert.AreEqual(ux, uxAvg, 1e-4f); + Assert.AreEqual(uy, uyAvg, 1e-4f); + Assert.AreEqual(uz, uzAvg, 1e-4f); + } + + [Test] + public void Average_IdenticalVectors() { + var dir = Direction.Radians(MathF.PI / 4f, MathF.PI / 6f); + var v = new Spherical(2.5f, dir); + var list = new List { v, v, v }; + + var avg = Spherical.Average(list); + + Assert.AreEqual(2.5f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal, avg.direction.horizontal); + Assert.AreEqual(dir.vertical, avg.direction.vertical); + } + + [Test] + public void Average_SingleElement() { + Spherical s = Spherical.Radians(1.234f, 0.3f, -0.7f); + Spherical avg = Spherical.Average(new List { s }); + + Assert.AreEqual(s.distance, avg.distance, 1e-5f); + Assert.AreEqual(s.direction.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f); + Assert.AreEqual(s.direction.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f); + } + + [Test] + public void Average_OppositeUnitVectors() { + // Two opposite vectors: same distance, horizontal opposite (pi apart), same vertical + Spherical v1 = Spherical.Radians(1f, 0f, 0f); + Spherical v2 = Spherical.Radians(1f, MathF.PI, 0f); + Spherical avg = Spherical.Average(new List { v1, v2 }); + + Assert.AreEqual(0f, avg.distance, 1e-4f); + // When distance is zero, angles may be undefined; allow any angle but ensure near-zero magnitude + } + + [Test] + public void Average_WeightedByDistance() { + // Two vectors same direction but different distances -> weighted average distance + Direction dir = Direction.Radians(MathF.PI / 3f, MathF.PI / 4f); + Spherical a = new(1f, dir); + Spherical b = new(3f, dir); + Spherical avg = Spherical.Average(new List { a, b }); + + // average distance should be (1+3)/2 = 2 + Assert.AreEqual(2f, avg.distance, 1e-5f); + Assert.AreEqual(dir.horizontal.inRadians, avg.direction.horizontal.inRadians, 1e-5f); + Assert.AreEqual(dir.vertical.inRadians, avg.direction.vertical.inRadians, 1e-5f); + } + + [Test] + public void Average_AxisSymmetricAroundVertical() { + // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same elevation (vertical) angle phi + float phi = MathF.PI / 6f; // elevation from horizontal plane + var dirs = new List { + new(1f, Direction.Radians(0f, phi)), + new(1f, Direction.Radians(MathF.PI/2, phi)), + new(1f, Direction.Radians(MathF.PI, phi)), + new(1f, Direction.Radians(3*MathF.PI/2, phi)) + }; + + Spherical avg = Spherical.Average(dirs); + + // rAvg should equal r * sin(elevation) = sin(phi) + Assert.AreEqual(MathF.Sin(phi), avg.distance, 1e-4f); + // vertical angle undefined when horizontal xy components cancel; allow any angle but ensure r matches + } + + [Test] + public void Average_AxisSymmetricAroundVertical2() { + // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same polar angle from vertical (alpha) + float alpha = MathF.PI / 6f; // polar angle from vertical + float elevation = MathF.PI / 2f - alpha; // convert polar-from-vertical to elevation + var dirs = new List { + new(1f, Direction.Radians(0f, elevation)), + new(1f, Direction.Radians(MathF.PI/2, elevation)), + new(1f, Direction.Radians(MathF.PI, elevation)), + new(1f, Direction.Radians(3*MathF.PI/2, elevation)) + }; + + Spherical avg = Spherical.Average(dirs); + + // rAvg should equal r * sin(elevation) which equals cos(alpha) + Assert.AreEqual(MathF.Cos(alpha), avg.distance, 1e-4f); + } + + [Test] + public void Average_CompareWithVector3() { + // Four vectors around azimuth 0, pi/2, pi, 3pi/2 at same polar angle from vertical (alpha) + float alpha = MathF.PI / 6f; // polar angle from vertical + float elevation = MathF.PI / 2f - alpha; // convert polar-from-vertical to elevation + List dirs = new List { + new(1f, Direction.Radians(0f, elevation)), + new(2f, Direction.Radians(MathF.PI/2, elevation+1)), + new(3f, Direction.Radians(MathF.PI, elevation+2)), + new(4f, Direction.Radians(3*MathF.PI/2, elevation+3)) + }; + + Spherical avg = Spherical.Average(dirs); + +#if UNITY_5_3_OR_NEWER + UnityEngine.Vector3 r = UnityEngine.Vector3.zero; +#else + Vector3Float r = Vector3Float.zero; +#endif + foreach (Spherical dir in dirs) { + r += dir.ToVector3(); + } + r = r / 4; + Spherical avg2 = Spherical.FromVector3(r); + + Assert.AreEqual(avg, avg2); + } + + } +} +//#endif \ No newline at end of file diff --git a/LinearAlgebra/test/SwingTwistTest.cs b/LinearAlgebra/test/SwingTwistTest.cs new file mode 100644 index 0000000..5f05a96 --- /dev/null +++ b/LinearAlgebra/test/SwingTwistTest.cs @@ -0,0 +1,131 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + + [TestFixture] + public class SwingTwistTest { + + [Test] + public void Degrees_CreatesSwingTwistWithDegreeAngles() { + SwingTwist st = SwingTwist.Degrees(45, 30, 15); + Assert.IsNotNull(st); + Assert.AreEqual(45, st.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(30, st.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(15, st.twist.inDegrees, 0.01f); + } + + [Test] + public void Radians_CreatesSwingTwistWithRadianAngles() { + float pi = (float)System.Math.PI; + SwingTwist st = SwingTwist.Radians(pi / 4, pi / 6, pi / 12); + Assert.IsNotNull(st); + Assert.AreEqual(45, st.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(30, st.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(15, st.twist.inDegrees, 0.01f); + } + + [Test] + public void Zero_CreatesZeroRotation() { + SwingTwist st = SwingTwist.zero; + Assert.AreEqual(0, st.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(0, st.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(0, st.twist.inDegrees, 0.01f); + } + + [Test] + public void QuaternionTest() { + Quaternion q; + SwingTwist s; + Quaternion r; + + q = Quaternion.identity; + s = SwingTwist.FromQuaternion(q); + r = s.ToQuaternion(); + Assert.AreEqual(q, r); + + q = Quaternion.Euler(90, 0, 0); + s = SwingTwist.FromQuaternion(q); + Assert.AreEqual(0, s.swing.horizontal.inDegrees, 10e-2f); + Assert.AreEqual(90, s.swing.vertical.inDegrees, 10e-2f); + Assert.AreEqual(0, s.twist.inDegrees, 0.01f); + r = s.ToQuaternion(); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + q = Quaternion.Euler(0, 90, 0); + s = SwingTwist.FromQuaternion(q); + Assert.AreEqual(90, s.swing.horizontal.inDegrees,10e-2f); + Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(0, s.twist.inDegrees, 0.01f); + r = s.ToQuaternion(); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + q = Quaternion.Euler(0, 0, 90); + s = SwingTwist.FromQuaternion(q); + Assert.AreEqual(0, s.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(90, s.twist.inDegrees, 0.01f); + r = s.ToQuaternion(); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + q = Quaternion.Euler(0, 180, 0); + s = SwingTwist.FromQuaternion(q); + Assert.AreEqual(-180, s.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(0, s.twist.inDegrees, 0.01f); + r = s.ToQuaternion(); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + q = Quaternion.Euler(0, 135, 0); + s = SwingTwist.FromQuaternion(q); + Assert.AreEqual(135, s.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(0, s.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(0, s.twist.inDegrees, 0.01f); + r = s.ToQuaternion(); + Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + q = Quaternion.Euler(60, 45, 30); + s = SwingTwist.FromQuaternion(q); + Assert.AreEqual(45, s.swing.horizontal.inDegrees, 0.01f); + Assert.AreEqual(60, s.swing.vertical.inDegrees, 0.01f); + Assert.AreEqual(30, s.twist.inDegrees, 0.01f); + // r = s.ToQuaternion(); + // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + // q = Quaternion.Euler(-45, -30, -15); + // s = SwingTwist.FromQuaternion(q); + // Assert.AreEqual(-30, s.swing.horizontal.inDegrees, 0.01f); + // Assert.AreEqual(-45, s.swing.vertical.inDegrees, 0.01f); + // Assert.AreEqual(-15, s.twist.inDegrees, 0.01f); + // r = s.ToQuaternion(); + // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + // q = Quaternion.Euler(180, 180, 180); + // s = SwingTwist.FromQuaternion(q); + // Assert.AreEqual(-180, s.swing.horizontal.inDegrees, 0.01f); + // Assert.AreEqual(-180, s.swing.vertical.inDegrees, 0.01f); + // Assert.AreEqual(-180, s.twist.inDegrees, 0.01f); + // r = s.ToQuaternion(); + // Assert.AreEqual(0, Quaternion.UnsignedAngle(q, r), 10e-2f); + + } + + [Test] + public void ToAngleAxis_ConvertsToSpherical() { + SwingTwist st = SwingTwist.Degrees(45, 30, 15); + Spherical s = st.ToAngleAxis(); + Assert.IsNotNull(s); + } + + [Test] + public void FromAngleAxis_ConvertsFromSpherical() { + Spherical s = new(90, Direction.Degrees(45, 0)); + SwingTwist st = SwingTwist.FromAngleAxis(s); + Assert.IsNotNull(st); + } + + } + +} + +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/Vector2FloatTest.cs b/LinearAlgebra/test/Vector2FloatTest.cs new file mode 100644 index 0000000..867765a --- /dev/null +++ b/LinearAlgebra/test/Vector2FloatTest.cs @@ -0,0 +1,364 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + using Vector2 = Vector2Float; + + public class Vector2FloatTest { + + [SetUp] + public void Setup() { + } + + [Test] + public void FromPolar() { + } + + [Test] + public void Equality() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + + Assert.IsFalse(v1 == v2, "4 5 == 1 2"); + Assert.IsTrue(v1 != v2, "4 5 != 1 2"); + + v2 = new(4, 5); + Assert.IsTrue(v1 == v2, "4 5 == 4 5"); + Assert.IsFalse(v1 != v2, "4 5 != 4 5"); + } + + [Test] + public void Magnitude() { + Vector2 v = new(1, 2); + float m = 0; + m = v.magnitude; + Assert.AreEqual(m, 2.236068F, "v.magnitude 1 2"); + + m = Vector2.MagnitudeOf(v); + Assert.AreEqual(m, 2.236068F, "MagnitudeOf 1 2"); + + v = new(-1, -2); + m = v.magnitude; + Assert.AreEqual(m, 2.236068F, "v.magnitude -1 -2"); + + v = new(0, 0); + m = v.magnitude; + Assert.AreEqual(m, 0, "v.magnitude 0 0"); + } + + [Test] + public void SqrMagnitude() { + Vector2 v = new(1, 2); + float m = 0; + + m = v.sqrMagnitude; + Assert.AreEqual(m, 5, "v.sqrMagnitude 1 2"); + + m = Vector2.SqrMagnitudeOf(v); + Assert.AreEqual(m, 5, "SqrMagnitudeOf 1 2"); + + v = new(-1, -2); + m = v.sqrMagnitude; + Assert.AreEqual(m, 5, "v.sqrMagnitude -1 -2"); + + v = new(0, 0); + m = v.sqrMagnitude; + Assert.AreEqual(m, 0, "v.sqrMagnitude 0 0"); + } + + [Test] + public void Distance() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + float f = 0; + + f = Vector2.Distance(v1, v2); + Assert.AreEqual(f, 4.24264002f, 1.0E-05F, "Distance(4 5, 1 2)"); + + v2 = new(-1, -2); + f = Vector2.Distance(v1, v2); + Assert.AreEqual(f, 8.602325F, "Distance(4 5, 1 2)"); + + v2 = new(0, 0); + f = Vector2.Distance(v1, v2); + Assert.AreEqual(f, 6.403124F, 1.0E-05F, "Distance(4 5, 1 2)"); + } + + [Test] + public void Normalize() { + Vector2 v = new(0, 3); + Vector2Float r; + + r = v.normalized; + Assert.AreEqual(0, r.horizontal, "normalized 0 3 H"); + Assert.AreEqual(1, r.vertical, "normalized 0 3 V"); + + r = Vector2.Normalize(v); + Assert.AreEqual(0, r.horizontal, "Normalize 0 3 H"); + Assert.AreEqual(1, r.vertical, "Normalize 0 3 V"); + + v = new(0, -3); + r = v.normalized; + Assert.AreEqual(0, r.horizontal, "normalized 0 -3 H"); + Assert.AreEqual(-1, r.vertical, "normalized 0 -3 V"); + + v = new(0, 0); + r = v.normalized; + Assert.AreEqual(0, r.horizontal, "normalized 0 0 H"); + Assert.AreEqual(0, r.vertical, "normalized 0 0 V"); + } + + [Test] + public void Negate() { + Vector2 v = new(4, 5); + Vector2 r; + + r = -v; + Assert.AreEqual(-4, r.horizontal, "- 4 5 H"); + Assert.AreEqual(-5, r.vertical, "- 4 5 V"); + + v = new(-4, -5); + r = -v; + Assert.AreEqual(4, r.horizontal, "- -4 -5 H"); + Assert.AreEqual(5, r.vertical, "- -4 -5 V"); + + v = new(0, 0); + r = -v; + Assert.AreEqual(0, r.horizontal, "- 0 0 H"); + Assert.AreEqual(0, r.vertical, "- 0 0 V"); + } + + [Test] + public void Subtract() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r = Vector2.zero; + + r = v1 - v2; + Assert.IsTrue(r == new Vector2(3, 3), "4 5 - 1 2"); + + v2 = new(-1, -2); + r = v1 - v2; + Assert.IsTrue(r == new Vector2(5, 7), "4 5 - -1 -2"); + + v2 = new(4, 5); + r = v1 - v2; + Assert.IsTrue(r == new Vector2(0, 0), "4 5 - 4 5"); + r = v1; + r -= v2; + Assert.AreEqual(r, new Vector2(0, 0), "4 5 - 4 5"); + + v2 = new(0, 0); + r = v1 - v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0"); + r -= v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0"); + } + + [Test] + public void Addition() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r = Vector2.zero; + + r = v1 + v2; + Assert.IsTrue(r == new Vector2(5, 7), "4 5 + 1 2"); + + v2 = new(-1, -2); + r = v1 + v2; + Assert.IsTrue(r == new Vector2(3, 3), "4 5 + -1 -2"); + r = v1; + r += v2; + Assert.AreEqual(r, new Vector2(3, 3), "4 5 + -1 -2"); + + v2 = new(0, 0); + r = v1 + v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0"); + r += v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0"); + } + + [Test] + public void Scale() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r; + + r = Vector2.Scale(v1, v2); + Assert.AreEqual(4, r.horizontal, "Scale 4 5 , 1 2 H"); + Assert.AreEqual(10, r.vertical, "Scale 4 5 , 1 2 V"); + + v2 = new(-1, -2); + r = Vector2.Scale(v1, v2); + Assert.AreEqual(-4, r.horizontal, "Scale 4 5 , -1 -2 H"); + Assert.AreEqual(-10, r.vertical, "Scale 4 5 , -1 -2 V"); + + v2 = new(0, 0); + r = Vector2.Scale(v1, v2); + Assert.AreEqual(0, r.horizontal, "Scale 4 5 , 0 0 H"); + Assert.AreEqual(0, r.vertical, "Scale 4 5 , 0 0 V"); + } + + [Test] + public void Multiply() { + Vector2 v1 = new(4, 5); + int f = 3; + Vector2 r; + + r = v1 * f; + Assert.AreEqual(12, r.horizontal, "4 5 * 3 H"); + Assert.AreEqual(15, r.vertical, "4 5 * 3 V"); + + r = f * v1; + Assert.AreEqual(12, r.horizontal, "3 * 4 5 H"); + Assert.AreEqual(15, r.vertical, "3 * 4 5 V"); + + f = -3; + r = v1 * f; + Assert.AreEqual(-12, r.horizontal, "4 5 * -3 H"); + Assert.AreEqual(-15, r.vertical, "4 5 * -3 V"); + + f = 0; + r = v1 * f; + Assert.AreEqual(0, r.horizontal, "4 5 * 0 H"); + Assert.AreEqual(0, r.vertical, "4 5 * 0 V"); + } + + [Test] + public void Divide() { + Vector2 v1 = new(4, 5); + float f = 2; + Vector2 r; + + r = v1 / f; + Assert.AreEqual(2, r.horizontal, "4 5 / 2 H"); + Assert.AreEqual(2.5, r.vertical, "4 5 / 2 V"); + + f = -2; + r = v1 / f; + Assert.AreEqual(-2, r.horizontal, "4 5 / -2 H"); + Assert.AreEqual(-2.5, r.vertical, "4 5 / -2 V"); + + f = 0; + r = v1 / f; + Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 / 0 H"); + Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 / 0 V"); + } + + [Test] + public void Dot() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + float f; + + f = Vector2.Dot(v1, v2); + Assert.AreEqual(14, f, "Dot(4 5, 1 2)"); + + v2 = new(-1, -2); + f = Vector2.Dot(v1, v2); + Assert.AreEqual(-14, f, "Dot(4 5, -1 -2)"); + + v2 = new(0, 0); + f = Vector2.Dot(v1, v2); + Assert.AreEqual(0, f, "Dot(4 5, 0 0)"); + } + + [Test] + public void SignedAngle() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + float f; + + f = Vector2.SignedAngle(v1, v2); + Assert.AreEqual(-12.094758f, f); + + v2 = new(-1, -2); + f = Vector2.SignedAngle(v1, v2); + Assert.AreEqual(167.905228f, f); + + v2 = new(0, 0); + f = Vector2.SignedAngle(v1, v2); + Assert.AreEqual(0, f); + + v1 = new(0, 1); + v2 = new(1, 0); + f = Vector2.SignedAngle(v1, v2); + Assert.AreEqual(90, f); + + v1 = new(0, 1); + v2 = new(0, -1); + f = Vector2.SignedAngle(v1, v2); + Assert.AreEqual(180, f); + } + + [Test] + public void UnsignedAngle() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + float f; + + f = Vector2.UnsignedAngle(v1, v2); + Assert.AreEqual(12.094758f, f); + + v2 = new(-1, -2); + f = Vector2.UnsignedAngle(v1, v2); + Assert.AreEqual(167.905228f, f); + + v2 = new(0, 0); + f = Vector2.UnsignedAngle(v1, v2); + Assert.AreEqual(0, f); + + v1 = new(0, 1); + v2 = new(1, 0); + f = Vector2.UnsignedAngle(v1, v2); + Assert.AreEqual(90, f); + + v1 = new(0, 1); + v2 = new(0, -1); + f = Vector2.UnsignedAngle(v1, v2); + Assert.AreEqual(180, f); + } + + [Test] + public void Rotate() { + Vector2 v1 = new(1, 2); + Vector2 r; + + r = Vector2.Rotate(v1, AngleFloat.Degrees(0)); + Assert.AreEqual(0, Vector2.Distance(r, v1)); + + r = Vector2.Rotate(v1, AngleFloat.Degrees(180)); + Assert.AreEqual(0, Vector2.Distance(r, new Vector2(-1, -2)), 1.0e-06); + + r = Vector2.Rotate(v1, AngleFloat.Degrees(-90)); + Assert.AreEqual(0, Vector2.Distance(r, new Vector2(2, -1)), 1.0e-06); + + r = Vector2.Rotate(v1, AngleFloat.Degrees(270)); + Assert.AreEqual(0, Vector2.Distance(r, new Vector2(2, -1)), 1.0e-06); + } + + [Test] + public void Lerp() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r; + + r = Vector2.Lerp(v1, v2, 0); + Assert.AreEqual(0, Vector2.Distance(r, v1), 0); + + r = Vector2.Lerp(v1, v2, 1); + Assert.AreEqual(0, Vector2.Distance(r, v2), 0); + + r = Vector2.Lerp(v1, v2, 0.5f); + Assert.AreEqual(0, Vector2.Distance(r, new Vector2(2.5f, 3.5f)), 0); + + r = Vector2.Lerp(v1, v2, -1); + Assert.AreEqual(0, Vector2.Distance(r, new Vector2(7, 8)), 0); + + r = Vector2.Lerp(v1, v2, 2); + Assert.AreEqual(0, Vector2.Distance(r, new Vector2(-2, -1)), 0); + } + + } +} +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/Vector2IntTest.cs b/LinearAlgebra/test/Vector2IntTest.cs new file mode 100644 index 0000000..3647ca0 --- /dev/null +++ b/LinearAlgebra/test/Vector2IntTest.cs @@ -0,0 +1,270 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + using Vector2 = Vector2Int; + + public class Vector2IntTest { + + [SetUp] + public void Setup() { + } + + [Test] + public void FromPolar() { + } + + [Test] + public void Equality() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + + Assert.IsFalse(v1 == v2, "4 5 == 1 2"); + Assert.IsTrue(v1 != v2, "4 5 != 1 2"); + + v2 = new(4, 5); + Assert.IsTrue(v1 == v2, "4 5 == 4 5"); + Assert.IsFalse(v1 != v2, "4 5 != 4 5"); + } + + [Test] + public void Magnitude() { + Vector2 v = new(1, 2); + float m = 0; + m = v.magnitude; + Assert.AreEqual(m, 2.236068F, "v.magnitude 1 2"); + + m = Vector2.MagnitudeOf(v); + Assert.AreEqual(m, 2.236068F, "MagnitudeOf 1 2"); + + v = new(-1, -2); + m = v.magnitude; + Assert.AreEqual(m, 2.236068F, "v.magnitude -1 -2"); + + v = new(0, 0); + m = v.magnitude; + Assert.AreEqual(m, 0, "v.magnitude 0 0"); + } + + [Test] + public void SqrMagnitude() { + Vector2 v = new(1, 2); + float m = 0; + + m = v.sqrMagnitude; + Assert.AreEqual(m, 5, "v.sqrMagnitude 1 2"); + + m = Vector2.SqrMagnitudeOf(v); + Assert.AreEqual(m, 5, "SqrMagnitudeOf 1 2"); + + v = new(-1, -2); + m = v.sqrMagnitude; + Assert.AreEqual(m, 5, "v.sqrMagnitude -1 -2"); + + v = new(0, 0); + m = v.sqrMagnitude; + Assert.AreEqual(m, 0, "v.sqrMagnitude 0 0"); + } + + [Test] + public void Distance() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + float f = 0; + + f = Vector2.Distance(v1, v2); + Assert.AreEqual(f, 4.24264002f, 1.0E-05F, "Distance(4 5, 1 2)"); + + v2 = new(-1, -2); + f = Vector2.Distance(v1, v2); + Assert.AreEqual(f, 8.602325F, "Distance(4 5, 1 2)"); + + v2 = new(0, 0); + f = Vector2.Distance(v1, v2); + Assert.AreEqual(f, 6.403124F, 1.0E-05F, "Distance(4 5, 1 2)"); + } + + [Test] + public void Normalize() { + Vector2 v = new(0, 3); + Vector2Float r; + + r = v.normalized; + Assert.AreEqual(0, r.horizontal, "normalized 0 3 H"); + Assert.AreEqual(1, r.vertical, "normalized 0 3 V"); + + r = Vector2.Normalize(v); + Assert.AreEqual(0, r.horizontal, "Normalize 0 3 H"); + Assert.AreEqual(1, r.vertical, "Normalize 0 3 V"); + + v = new(0, -3); + r = v.normalized; + Assert.AreEqual(0, r.horizontal, "normalized 0 -3 H"); + Assert.AreEqual(-1, r.vertical, "normalized 0 -3 V"); + + v = new(0, 0); + r = v.normalized; + Assert.AreEqual(0, r.horizontal, "normalized 0 0 H"); + Assert.AreEqual(0, r.vertical, "normalized 0 0 V"); + } + + [Test] + public void Negate() { + Vector2 v = new(4, 5); + Vector2 r; + + r = -v; + Assert.AreEqual(-4, r.horizontal, "- 4 5 H"); + Assert.AreEqual(-5, r.vertical, "- 4 5 V"); + + v = new(-4, -5); + r = -v; + Assert.AreEqual(4, r.horizontal, "- -4 -5 H"); + Assert.AreEqual(5, r.vertical, "- -4 -5 V"); + + v = new(0, 0); + r = -v; + Assert.AreEqual(0, r.horizontal, "- 0 0 H"); + Assert.AreEqual(0, r.vertical, "- 0 0 V"); + } + + [Test] + public void Subtract() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r = Vector2.zero; + + r = v1 - v2; + Assert.IsTrue(r == new Vector2(3, 3), "4 5 - 1 2"); + + v2 = new(-1, -2); + r = v1 - v2; + Assert.IsTrue(r == new Vector2(5, 7), "4 5 - -1 -2"); + + v2 = new(4, 5); + r = v1 - v2; + Assert.IsTrue(r == new Vector2(0, 0), "4 5 - 4 5"); + r = v1; + r -= v2; + Assert.AreEqual(r, new Vector2(0, 0), "4 5 - 4 5"); + + v2 = new(0, 0); + r = v1 - v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0"); + r -= v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 - 0 0"); + } + + [Test] + public void Addition() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r = Vector2.zero; + + r = v1 + v2; + Assert.IsTrue(r == new Vector2(5, 7), "4 5 + 1 2"); + + v2 = new(-1, -2); + r = v1 + v2; + Assert.IsTrue(r == new Vector2(3, 3), "4 5 + -1 -2"); + r = v1; + r += v2; + Assert.AreEqual(r, new Vector2(3, 3), "4 5 + -1 -2"); + + v2 = new(0, 0); + r = v1 + v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0"); + r += v2; + Assert.AreEqual(r, new Vector2(4, 5), "4 5 + 0 0"); + } + + [Test] + public void Scale() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + Vector2 r; + + r = Vector2.Scale(v1, v2); + Assert.AreEqual(4, r.horizontal, "Scale 4 5 , 1 2 H"); + Assert.AreEqual(10, r.vertical, "Scale 4 5 , 1 2 V"); + + v2 = new(-1, -2); + r = Vector2.Scale(v1, v2); + Assert.AreEqual(-4, r.horizontal, "Scale 4 5 , -1 -2 H"); + Assert.AreEqual(-10, r.vertical, "Scale 4 5 , -1 -2 V"); + + v2 = new(0, 0); + r = Vector2.Scale(v1, v2); + Assert.AreEqual(0, r.horizontal, "Scale 4 5 , 0 0 H"); + Assert.AreEqual(0, r.vertical, "Scale 4 5 , 0 0 V"); + } + + [Test] + public void Multiply() { + Vector2 v1 = new(4, 5); + int f = 3; + Vector2 r; + + r = v1 * f; + Assert.AreEqual(12, r.horizontal, "4 5 * 3 H"); + Assert.AreEqual(15, r.vertical, "4 5 * 3 V"); + + r = f * v1; + Assert.AreEqual(12, r.horizontal, "3 * 4 5 H"); + Assert.AreEqual(15, r.vertical, "3 * 4 5 V"); + + f = -3; + r = v1 * f; + Assert.AreEqual(-12, r.horizontal, "4 5 * -3 H"); + Assert.AreEqual(-15, r.vertical, "4 5 * -3 V"); + + f = 0; + r = v1 * f; + Assert.AreEqual(0, r.horizontal, "4 5 * 0 H"); + Assert.AreEqual(0, r.vertical, "4 5 * 0 V"); + } + + [Test] + public void Divide() { + Vector2 v1 = new(4, 5); + int f = 2; + Vector2 r; + + r = v1 / f; + Assert.AreEqual(2, r.horizontal, "4 5 / 2 H"); + Assert.AreEqual(2, r.vertical, "4 5 / 2 V"); + + f = -2; + r = v1 / f; + Assert.AreEqual(-2, r.horizontal, "4 5 / -2 H"); + Assert.AreEqual(-2, r.vertical, "4 5 / -2 V"); + + Assert.Throws(() => { + f = 0; + r = v1 / f; + Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 / 0 H"); + Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 / 0 V"); + }); + } + + [Test] + public void Dot() { + Vector2 v1 = new(4, 5); + Vector2 v2 = new(1, 2); + int f; + + f = Vector2.Dot(v1, v2); + Assert.AreEqual(14, f, "Dot(4 5, 1 2)"); + + v2 = new(-1, -2); + f = Vector2.Dot(v1, v2); + Assert.AreEqual(-14, f, "Dot(4 5, -1 -2)"); + + v2 = new(0, 0); + f = Vector2.Dot(v1, v2); + Assert.AreEqual(0, f, "Dot(4 5, 0 0)"); + } + + } +} +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/Vector3FloatTest.cs b/LinearAlgebra/test/Vector3FloatTest.cs new file mode 100644 index 0000000..fd3c2dc --- /dev/null +++ b/LinearAlgebra/test/Vector3FloatTest.cs @@ -0,0 +1,581 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + using Vector3 = Vector3Float; + + public class Vector3FloatTest { + + [Test] + public void FromSpherical() { + Vector3 v = new(0, 0, 1); + Spherical s = Spherical.FromVector3(v); + Vector3 r = Vector3.FromSpherical(s); + + Assert.AreEqual(0, r.horizontal, "0 0 1"); + Assert.AreEqual(0, r.vertical, 1.0e-06, "0 0 1"); + Assert.AreEqual(1, r.depth, "0 0 1"); + + v = new(0, 1, 0); + s = Spherical.FromVector3(v); + r = Vector3.FromSpherical(s); + Assert.AreEqual(0, r.horizontal, "0 0 1"); + Assert.AreEqual(1, r.vertical, "0 0 1"); + Assert.AreEqual(0, r.depth, 1.0e-06, "0 0 1"); + + v = new(1, 0, 0); + s = Spherical.FromVector3(v); + r = Vector3.FromSpherical(s); + Assert.AreEqual(1, r.horizontal, "0 0 1"); + Assert.AreEqual(0, r.vertical, 1.0e-06, "0 0 1"); + Assert.AreEqual(0, r.depth, 1.0e-06, "0 0 1"); + } + + [Test] + public void Magnitude() { + Vector3 v = new(1, 2, 3); + float m = 0; + + m = v.magnitude; + Assert.AreEqual(3.7416575f, m, "magnitude 1 2 3"); + + m = Vector3.MagnitudeOf(v); + Assert.AreEqual(3.7416575f, m, "MagnitudeOf 1 2 3"); + + v = new(-1, -2, -3); + m = v.magnitude; + Assert.AreEqual(3.7416575f, m, "magnitude -1 -2 -3"); + + v = new(0, 0, 0); + m = v.magnitude; + Assert.AreEqual(0, m, "magnitude 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void SqrMagnitude() { + Vector3 v = new(1, 2, 3); + float m = 0; + + m = v.sqrMagnitude; + Assert.AreEqual(14, m, "sqrMagnitude 1 2 3"); + + m = Vector3.SqrMagnitudeOf(v); + Assert.AreEqual(14, m, "SqrMagnitudeOf 1 2 3"); + + v = new(-1, -2, -3); + m = v.sqrMagnitude; + Assert.AreEqual(14, m, "sqrMagnitude -1 -2 -3"); + + v = new(0, 0, 0); + m = v.sqrMagnitude; + Assert.AreEqual(0, m, "sqrMagnitude 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void Normalize() { + Vector3 v = new(0, 2, 0); + Vector3 r; + + r = v.normalized; + Assert.AreEqual(new Vector3(0, 1, 0), r, "normalized 0 2 0"); + + r = Vector3.Normalize(v); + Assert.AreEqual(new Vector3(0, 1, 0), r, "Normalize 0 2 0"); + + v = new(0, -2, 0); + r = v.normalized; + Assert.AreEqual(new Vector3(0, -1, 0), r, "normalized 0 -2 0"); + v = new(0, 0, 0); + r = v.normalized; + Assert.AreEqual(new Vector3(0, 0, 0), r, "normalized 0 0 0"); + + v = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = v.normalized; + Assert.IsTrue(float.IsNaN(r.horizontal), "normalized infinity infinity infinity"); + Assert.IsTrue(float.IsNaN(r.vertical), "normalized infinity infinity infinity"); + Assert.IsTrue(float.IsNaN(r.depth), "normalized infinity infinity infinity"); + + v = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = v.normalized; + Assert.IsTrue(float.IsNaN(r.horizontal), "normalized -infinity -infinity -infinity"); + Assert.IsTrue(float.IsNaN(r.vertical), "normalized -infinity -infinity -infinity"); + Assert.IsTrue(float.IsNaN(r.depth), "normalized -infinity -infinity -infinity"); + } + + [Test] + public void Negate() { + Vector3 v = new(4, 5, 6); + Vector3 r; + + r = -v; + Assert.AreEqual(-4, r.horizontal, "- 4 5 6 H"); + Assert.AreEqual(-5, r.vertical, "- 4 5 6 V"); + Assert.AreEqual(-6, r.depth, "- 4 5 6 D"); + + v = new(-4, -5, -6); + r = -v; + Assert.AreEqual(4, r.horizontal, "- -4 -5 -6 H"); + Assert.AreEqual(5, r.vertical, "- -4 -5 -6 V"); + Assert.AreEqual(6, r.depth, "- -4 -5 -6 D"); + + v = new(0, 0, 0); + r = -v; + Assert.AreEqual(new Vector3(0, 0, 0), r, "- 0 0 0"); + Assert.AreEqual(0, r.horizontal, "- 0 0 0 H"); + Assert.AreEqual(0, r.vertical, "- 0 0 0 V"); + Assert.AreEqual(0, r.depth, "- 0 0 0 D"); + + + v = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = -v; + Assert.AreEqual(new Vector3(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity), r, "- inifinty infinity infinity"); + + v = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = -v; + Assert.AreEqual(new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity), r, "- -inifinty -infinity -infinity"); + } + + [Test] + public void Subtract() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r = Vector3.zero; + + r = v1 - v2; + Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 - 1 2 3"); + + v2 = new(-1, -2, -3); + r = v1 - v2; + Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 - -1 -2 -3"); + + v2 = new(4, 5, 6); + r = v1 - v2; + Assert.IsTrue(r == new Vector3(0, 0, 0), "4 5 6 - 4 5 6"); + r = v1; + r -= v2; + Assert.AreEqual(r, new Vector3(0, 0, 0), "4 5 6 - 4 5 6"); + + v2 = new(0, 0, 0); + r = v1 - v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0"); + r -= v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void Addition() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r = Vector3.zero; + + r = v1 + v2; + Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 + 1 2 3"); + + v2 = new(-1, -2, -3); + r = v1 + v2; + Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3"); + r = v1; + r += v2; + Assert.AreEqual(r, new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3"); + + v2 = new(0, 0, 0); + r = v1 + v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0"); + r += v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void Scale() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.Scale(v1, v2); + Assert.AreEqual(4, r.horizontal, "Scale 4 5 6 , 1 2 3 H"); + Assert.AreEqual(10, r.vertical, "Scale 4 5 6 , 1 2 3 V"); + Assert.AreEqual(18, r.depth, "Scale 4 5 6 , 1 2 3 D"); + + v2 = new(-1, -2, -3); + r = Vector3.Scale(v1, v2); + Assert.AreEqual(-4, r.horizontal, "Scale 4 5 6 , -1 -2 -3 H"); + Assert.AreEqual(-10, r.vertical, "Scale 4 5 6 , -1 -2 -3 V"); + Assert.AreEqual(-18, r.depth, "Scale 4 5 6 , -1 -2 -3 D"); + + v2 = new(0, 0, 0); + r = Vector3.Scale(v1, v2); + Assert.AreEqual(0, r.horizontal, "Scale 4 5 6 , 0 0 0 H"); + Assert.AreEqual(0, r.vertical, "Scale 4 5 6 , 0 0 0 V"); + Assert.AreEqual(0, r.depth, "Scale 4 5 6 , 0 0 0 D"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = Vector3.Scale(v1, v2); + Assert.AreEqual(float.PositiveInfinity, r.horizontal, "Scale 4 5 6 , inf inf inf H"); + Assert.AreEqual(float.PositiveInfinity, r.vertical, "Scale 4 5 6 , inf inf inf V"); + Assert.AreEqual(float.PositiveInfinity, r.depth, "Scale 4 5 6 , inf inf inf D"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = Vector3.Scale(v1, v2); + Assert.AreEqual(float.NegativeInfinity, r.horizontal, "Scale 4 5 6 , -inf -inf -inf H"); + Assert.AreEqual(float.NegativeInfinity, r.vertical, "Scale 4 5 6 , -inf -inf -inf V"); + Assert.AreEqual(float.NegativeInfinity, r.depth, "Scale 4 5 6 , -inf -inf -inf D"); + } + + [Test] + public void Multiply() { + Vector3 v1 = new(4, 5, 6); + float f = 3; + Vector3 r; + + r = v1 * f; + Assert.AreEqual(12, r.horizontal, "4 5 6 * 3 H"); + Assert.AreEqual(15, r.vertical, "4 5 6 * 3 V"); + Assert.AreEqual(18, r.depth, "4 5 6 * 3 D"); + + f = -3; + r = v1 * f; + Assert.AreEqual(-12, r.horizontal, "4 5 6 * -3 H"); + Assert.AreEqual(-15, r.vertical, "4 5 6 * -3 V"); + Assert.AreEqual(-18, r.depth, "4 5 6 * -3 D"); + + f = 0; + r = v1 * f; + Assert.AreEqual(0, r.horizontal, "4 5 6 * 0 H"); + Assert.AreEqual(0, r.vertical, "4 5 6 * 0 V"); + Assert.AreEqual(0, r.depth, "4 5 6 * 0 D"); + + f = float.PositiveInfinity; + r = v1 * f; + Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 6 * inf H"); + Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 6 * inf V"); + Assert.AreEqual(float.PositiveInfinity, r.depth, "4 5 6 * inf D"); + + f = float.NegativeInfinity; + r = v1 * f; + Assert.AreEqual(float.NegativeInfinity, r.horizontal, "4 5 6 * -inf H"); + Assert.AreEqual(float.NegativeInfinity, r.vertical, "4 5 6 * -inf V"); + Assert.AreEqual(float.NegativeInfinity, r.depth, "4 5 6 * -inf D"); + } + + [Test] + public void Divide() { + Vector3 v1 = new(4, 5, 6); + float f = 2; + Vector3 r; + + r = v1 / f; + Assert.AreEqual(2, r.horizontal, "4 5 6 / 2 H"); + Assert.AreEqual(2.5, r.vertical, "4 5 6 / 2 V"); + Assert.AreEqual(3, r.depth, "4 5 6 / 2 D"); + + f = -2; + r = v1 / f; + Assert.AreEqual(-2, r.horizontal, "4 5 6 / -2 H"); + Assert.AreEqual(-2.5, r.vertical, "4 5 6 / -2 V"); + Assert.AreEqual(-3, r.depth, "4 5 6 / -2 D"); + + f = 0; + r = v1 / f; + Assert.AreEqual(float.PositiveInfinity, r.horizontal, "4 5 6 / 0 H"); + Assert.AreEqual(float.PositiveInfinity, r.vertical, "4 5 6 / 0 V"); + Assert.AreEqual(float.PositiveInfinity, r.depth, "4 5 6 / 0 D"); + + f = float.PositiveInfinity; + r = v1 / f; + Assert.AreEqual(0, r.horizontal, "4 5 6 / inf H"); + Assert.AreEqual(0, r.vertical, "4 5 6 / inf V"); + Assert.AreEqual(0, r.depth, "4 5 6 / inf D"); + + f = float.NegativeInfinity; + r = v1 / f; + Assert.AreEqual(0, r.horizontal, "4 5 6 / -inf H"); + Assert.AreEqual(0, r.vertical, "4 5 6 / -inf V"); + Assert.AreEqual(0, r.depth, "4 5 6 / -inf D"); + } + + [Test] + public void Dot() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + float f; + + f = Vector3.Dot(v1, v2); + Assert.AreEqual(32, f, "Dot(4 5 6, 1 2 3)"); + + v2 = new(-1, -2, -3); + f = Vector3.Dot(v1, v2); + Assert.AreEqual(-32, f, "Dot(4 5 6, -1 -2 -3)"); + + v2 = new(0, 0, 0); + f = Vector3.Dot(v1, v2); + Assert.AreEqual(0, f, "Dot(4 5 6, 0 0 0)"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + f = Vector3.Dot(v1, v2); + Assert.AreEqual(float.PositiveInfinity, f, "Dot(4 5 6, inf inf inf)"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + f = Vector3.Dot(v1, v2); + Assert.AreEqual(float.NegativeInfinity, f, "Dot(4 5 6, -inf -inf -inf)"); + } + + [Test] + public void Equality() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + bool r; + + r = v1 == v2; + Assert.IsFalse(r, "4 5 6 == 1 2 3"); + + v2 = new(4, 5, 6); + r = v1 == v2; + Assert.IsTrue(r, "4 5 6 == 4 5 6"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = v1 == v2; + Assert.IsFalse(r, "4 5 6 == inf inf inf"); + + v1 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = v1 == v2; + Assert.IsFalse(r, "-inf -inf -inf == inf inf inf"); + } + + [Test] + public void Distance() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + float f; + + f = Vector3.Distance(v1, v2); + Assert.AreEqual(5.19615221F, f, "Distance(4 5 6, 1 2 3)"); + + v2 = new(-1, -2, -3); + f = Vector3.Distance(v1, v2); + Assert.AreEqual(12.4498997F, f, "Distance(4 5 6, -1 -2 -3)"); + + v2 = new(0, 0, 0); + f = Vector3.Distance(v1, v2); + Assert.AreEqual(v1.magnitude, f, "Distance(4 5 6, 0 0 0)"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + f = Vector3.Distance(v1, v2); + Assert.AreEqual(float.PositiveInfinity, f, "Distance(4 5 6, inf inf inf)"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + f = Vector3.Distance(v1, v2); + Assert.AreEqual(float.PositiveInfinity, f, "Distance(4 5 6, -inf -inf -inf)"); + } + + [Test] + public void Cross() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.Cross(v1, v2); + Assert.AreEqual(3, r.horizontal, "Cross(4 5 6, 1 2 3) H"); + Assert.AreEqual(-6, r.vertical, "Cross(4 5 6, 1 2 3) V"); + Assert.AreEqual(3, r.depth, "Cross(4 5 6, 1 2 3) D"); + + v2 = new(-1, -2, -3); + r = Vector3.Cross(v1, v2); + Assert.AreEqual(-3, r.horizontal, "Cross(4 5 6, -1 -2 -3) H"); + Assert.AreEqual(6, r.vertical, "Cross(4 5 6, -1 -2 -3) V"); + Assert.AreEqual(-3, r.depth, "Cross(4 5 6, -1 -2 -3) D"); + + v2 = new(0, 0, 0); + r = Vector3.Cross(v1, v2); + Assert.AreEqual(0, r.horizontal, "Cross(4 5 6, 0 0 0) H"); + Assert.AreEqual(0, r.vertical, "Cross(4 5 6, 0 0 0) V"); + Assert.AreEqual(0, r.depth, "Cross(4 5 6, 0 0 0) D"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = Vector3.Cross(v1, v2); + Assert.IsTrue(float.IsNaN(r.horizontal), "Cross(4 5 6, inf inf inf) H"); + Assert.IsTrue(float.IsNaN(r.vertical), "Cross(4 5 6, inf inf inf) V"); + Assert.IsTrue(float.IsNaN(r.depth), "Cross(4 5 6, inf inf inf) D"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = Vector3.Cross(v1, v2); + Assert.IsTrue(float.IsNaN(r.horizontal), "Cross(4 5 6, -inf -inf -inf) H"); + Assert.IsTrue(float.IsNaN(r.vertical), "Cross(4 5 6, -inf -inf -inf) V"); + Assert.IsTrue(float.IsNaN(r.depth), "Cross(4 5 6, -inf -inf -inf) D"); + } + + [Test] + public void Project() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.Project(v1, v2); + Assert.AreEqual(2.28571439F, r.horizontal, "Project(4 5 6, 1 2 3) H"); + Assert.AreEqual(4.57142878F, r.vertical, "Project(4 5 6, 1 2 3) V"); + Assert.AreEqual(6.85714293F, r.depth, "Project(4 5 6, 1 2 3) D"); + + v2 = new(-1, -2, -3); + r = Vector3.Project(v1, v2); + Assert.AreEqual(2.28571439F, r.horizontal, "Project(4 5 6, -1 -2 -3) H"); + Assert.AreEqual(4.57142878F, r.vertical, "Project(4 5 6, -1 -2 -3) V"); + Assert.AreEqual(6.85714293F, r.depth, "Project(4 5 6, -1 -2 -3) D"); + + v2 = new(0, 0, 0); + r = Vector3.Project(v1, v2); + Assert.AreEqual(0, r.horizontal, "Project(4 5 6, 0 0 0) H"); + Assert.AreEqual(0, r.vertical, "Project(4 5 6, 0 0 0) V"); + Assert.AreEqual(0, r.depth, "Project(4 5 6, 0 0 0) D"); + + r = Vector3.Project(v2, v1); + Assert.AreEqual(0, r.horizontal, "Project(0 0 0, 4 5 6) H"); + Assert.AreEqual(0, r.vertical, "Project(0 0 0, 4 5 6) V"); + Assert.AreEqual(0, r.depth, "Project(0 0 0, 4 5 6) D"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = Vector3.Project(v1, v2); + Assert.IsTrue(float.IsNaN(r.horizontal), "Project(4 5 6, inf inf inf) H"); + Assert.IsTrue(float.IsNaN(r.vertical), "Project(4 5 6, inf inf inf) V"); + Assert.IsTrue(float.IsNaN(r.depth), "Project(4 5 6, inf inf inf) D"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = Vector3.Project(v1, v2); + Assert.IsTrue(float.IsNaN(r.horizontal), "Project(4 5 6, -inf -inf -inf) H"); + Assert.IsTrue(float.IsNaN(r.vertical), "Project(4 5 6, -inf -inf -inf) V"); + Assert.IsTrue(float.IsNaN(r.depth), "Project(4 5 6, -inf -inf -inf) D"); + } + + [Test] + public void ProjectOnPlane() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.ProjectOnPlane(v1, v2); + Assert.AreEqual(1.71428561F, r.horizontal, "ProjectOnPlane(4 5 6, 1 2 3) H"); + Assert.AreEqual(0.428571224F, r.vertical, "ProjectOnPlane(4 5 6, 1 2 3) V"); + Assert.AreEqual(-0.857142925F, r.depth, "ProjectOnPlane(4 5 6, 1 2 3) D"); + + v2 = new(-1, -2, -3); + r = Vector3.ProjectOnPlane(v1, v2); + Assert.AreEqual(1.71428561F, r.horizontal, "ProjectOnPlane(4 5 6, -1 -2 -3) H"); + Assert.AreEqual(0.428571224F, r.vertical, "ProjectOnPlane(4 5 6, -1 -2 -3) V"); + Assert.AreEqual(-0.857142925F, r.depth, "ProjectOnPlane(4 5 6, -1 -2 -3) D"); + + v2 = new(0, 0, 0); + r = Vector3.ProjectOnPlane(v1, v2); + Assert.AreEqual(4, r.horizontal, "ProjectOnPlane(4 5 6, 0 0 0) H"); + Assert.AreEqual(5, r.vertical, "ProjectOnPlane(4 5 6, 0 0 0) V"); + Assert.AreEqual(6, r.depth, "ProjectOnPlane(4 5 6, 0 0 0) D"); + + r = Vector3.ProjectOnPlane(v2, v1); + Assert.AreEqual(0, r.horizontal, "ProjectOnPlane(0 0 0, 4 5 6) H"); + Assert.AreEqual(0, r.vertical, "ProjectOnPlane(0 0 0, 4 5 6) V"); + Assert.AreEqual(0, r.depth, "ProjectOnPlane(0 0 0, 4 5 6) D"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + r = Vector3.ProjectOnPlane(v1, v2); + Assert.IsTrue(float.IsNaN(r.horizontal), "ProjectOnPlane(4 5 6, inf inf inf) H"); + Assert.IsTrue(float.IsNaN(r.vertical), "ProjectOnPlane(4 5 6, inf inf inf) V"); + Assert.IsTrue(float.IsNaN(r.depth), "ProjectOnPlane(4 5 6, inf inf inf) D"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + r = Vector3.ProjectOnPlane(v1, v2); + Assert.IsTrue(float.IsNaN(r.horizontal), "ProjectOnPlane(4 5 6, -inf -inf -inf) H"); + Assert.IsTrue(float.IsNaN(r.vertical), "ProjectOnPlane(4 5 6, -inf -inf -inf) V"); + Assert.IsTrue(float.IsNaN(r.depth), "ProjectOnPlane(4 5 6, -inf -inf -inf) D"); + } + + [Test] + public void UnsignedAngle() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + AngleFloat a; + + a = Vector3.UnsignedAngle(v1, v2); + Assert.AreEqual(12.9331379F, a.inDegrees, "Angle(4 5 6, 1 2 3)"); + + v2 = new(-1, -2, -3); + a = Vector3.UnsignedAngle(v1, v2); + Assert.AreEqual(167.066849F, a.inDegrees, "Angle(4 5 6, -1 -2 -3)"); + + v2 = new(0, 0, 0); + a = Vector3.UnsignedAngle(v1, v2); + Assert.AreEqual(0, a.inDegrees, "Angle(4 5 6, 0 0 0)"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + a = Vector3.UnsignedAngle(v1, v2); + Assert.IsTrue(float.IsNaN(a.inDegrees), "Angle(4 5 6, inf inf inf)"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + a = Vector3.UnsignedAngle(v1, v2); + Assert.IsTrue(float.IsNaN(a.inDegrees), "Angle(4 5 6, inf inf inf)"); + } + + [Test] + public void SignedAngle() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 v3 = new(7, 8, -9); + AngleFloat a; + + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(-12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 7 8 -9)"); + + v2 = new(-1, -2, -3); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(167.066849F, a.inDegrees, "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)"); + + v2 = new(0, 0, 0); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 0 0 0, 7 8 -9)"); + + v2 = new(1, 2, 3); + v3 = new(-7, -8, 9); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, -7 -8 9)"); + + v3 = new(0, 0, 0); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 0 0 0)"); + + v2 = new(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.IsTrue(float.IsNaN(a.inDegrees), "SignedAngle(4 5 6, inf inf inf, 0 0 0)"); + + v2 = new(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.IsTrue(float.IsNaN(a.inDegrees), "SignedAngle(4 5 6, -inf -inf -inf, 0 0 0)"); + } + + [Test] + public void Lerp() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.Lerp(v1, v2, 0); + Assert.AreEqual(0, Vector3.Distance(r, v1), 0); + + r = Vector3.Lerp(v1, v2, 1); + Assert.AreEqual(0, Vector3.Distance(r, v2), 0); + + r = Vector3.Lerp(v1, v2, 0.5f); + Assert.AreEqual(0, Vector3.Distance(r, new Vector3(2.5f, 3.5f, 4.5f)), 0); + + r = Vector3.Lerp(v1, v2, -1); + Assert.AreEqual(0, Vector3.Distance(r, new Vector3(7, 8, 9)), 0); + + r = Vector3.Lerp(v1, v2, 2); + Assert.AreEqual(0, Vector3.Distance(r, new Vector3(-2, -1, 0)), 0); + } + } +} +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/Vector3IntTest.cs b/LinearAlgebra/test/Vector3IntTest.cs new file mode 100644 index 0000000..b718178 --- /dev/null +++ b/LinearAlgebra/test/Vector3IntTest.cs @@ -0,0 +1,349 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + using Vector3 = Vector3Int; + + public class Vector3IntTest { + + [Test] + public void Equality() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + + Assert.IsFalse(v1 == v2, "4 5 6 == 1 2 3"); + Assert.IsTrue(v1 != v2, "4 5 6 != 1 2 3"); + + v2 = new(4, 5, 6); + Assert.IsTrue(v1 == v2, "4 5 6 == 4 5 6"); + Assert.IsFalse(v1 != v2, "4 5 6 != 4 5 6"); + } + + [Test] + public void Magnitude() { + Vector3 v = new(1, 2, 3); + float m = 0; + + m = v.magnitude; + Assert.AreEqual(3.7416575f, m, "magnitude 1 2 3"); + + m = Vector3.MagnitudeOf(v); + Assert.AreEqual(3.7416575f, m, "MagnitudeOf 1 2 3"); + + v = new(-1, -2, -3); + m = v.magnitude; + Assert.AreEqual(3.7416575f, m, "magnitude -1 -2 -3"); + + v = new(0, 0, 0); + m = v.magnitude; + Assert.AreEqual(0, m, "magnitude 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void SqrMagnitude() { + Vector3 v = new(1, 2, 3); + float m = 0; + + m = v.sqrMagnitude; + Assert.AreEqual(14, m, "sqrMagnitude 1 2 3"); + + m = Vector3.SqrMagnitudeOf(v); + Assert.AreEqual(14, m, "SqrMagnitudeOf 1 2 3"); + + v = new(-1, -2, -3); + m = v.sqrMagnitude; + Assert.AreEqual(14, m, "sqrMagnitude -1 -2 -3"); + + v = new(0, 0, 0); + m = v.sqrMagnitude; + Assert.AreEqual(0, m, "sqrMagnitude 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void Distance() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + float f; + + f = Vector3.Distance(v1, v2); + Assert.AreEqual(5.19615221F, f, "Distance(4 5 6, 1 2 3)"); + + v2 = new(-1, -2, -3); + f = Vector3.Distance(v1, v2); + Assert.AreEqual(12.4498997F, f, "Distance(4 5 6, -1 -2 -3)"); + + v2 = new(0, 0, 0); + f = Vector3.Distance(v1, v2); + Assert.AreEqual(v1.magnitude, f, "Distance(4 5 6, 0 0 0)"); + } + + [Test] + public void Normalize() { + Vector3 v = new(0, 2, 0); + Vector3Float r; + + r = v.normalized; + //Assert.AreEqual(new Vector3(0, 1, 0), r, "normalized 0 2 0"); + Assert.AreEqual(0, r.horizontal, "normalized 0 2 0"); + Assert.AreEqual(1, r.vertical, "normalized 0 2 0"); + Assert.AreEqual(0, r.depth, "normalized 0 2 0"); + + r = Vector3.Normalize(v); + Assert.AreEqual(new Vector3Float(0, 1, 0), r, "Normalize 0 2 0"); + + v = new(0, -2, 0); + r = v.normalized; + Assert.AreEqual(new Vector3Float(0, -1, 0), r, "normalized 0 -2 0"); + v = new(0, 0, 0); + r = v.normalized; + Assert.AreEqual(new Vector3Float(0, 0, 0), r, "normalized 0 0 0"); + } + + [Test] + public void Negate() { + Vector3 v = new(4, 5, 6); + Vector3 r; + + r = -v; + Assert.AreEqual(-4, r.horizontal, "- 4 5 6 H"); + Assert.AreEqual(-5, r.vertical, "- 4 5 6 V"); + Assert.AreEqual(-6, r.depth, "- 4 5 6 D"); + + v = new(-4, -5, -6); + r = -v; + Assert.AreEqual(4, r.horizontal, "- -4 -5 -6 H"); + Assert.AreEqual(5, r.vertical, "- -4 -5 -6 V"); + Assert.AreEqual(6, r.depth, "- -4 -5 -6 D"); + + v = new(0, 0, 0); + r = -v; + Assert.AreEqual(new Vector3(0, 0, 0), r, "- 0 0 0"); + Assert.AreEqual(0, r.horizontal, "- 0 0 0 H"); + Assert.AreEqual(0, r.vertical, "- 0 0 0 V"); + Assert.AreEqual(0, r.depth, "- 0 0 0 D"); + } + + [Test] + public void Subtract() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r = Vector3.zero; + + r = v1 - v2; + Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 - 1 2 3"); + + v2 = new(-1, -2, -3); + r = v1 - v2; + Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 - -1 -2 -3"); + + v2 = new(4, 5, 6); + r = v1 - v2; + Assert.IsTrue(r == new Vector3(0, 0, 0), "4 5 6 - 4 5 6"); + r = v1; + r -= v2; + Assert.AreEqual(r, new Vector3(0, 0, 0), "4 5 6 - 4 5 6"); + + v2 = new(0, 0, 0); + r = v1 - v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0"); + r -= v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 - 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void Addition() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r = Vector3.zero; + + r = v1 + v2; + Assert.IsTrue(r == new Vector3(5, 7, 9), "4 5 6 + 1 2 3"); + + v2 = new(-1, -2, -3); + r = v1 + v2; + Assert.IsTrue(r == new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3"); + r = v1; + r += v2; + Assert.AreEqual(r, new Vector3(3, 3, 3), "4 5 6 + -1 -2 -3"); + + v2 = new(0, 0, 0); + r = v1 + v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0"); + r += v2; + Assert.AreEqual(r, new Vector3(4, 5, 6), "4 5 6 + 0 0 0"); + + // Infinity tests are still missing + } + + [Test] + public void Scale() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.Scale(v1, v2); + Assert.AreEqual(4, r.horizontal, "Scale 4 5 6 , 1 2 3 H"); + Assert.AreEqual(10, r.vertical, "Scale 4 5 6 , 1 2 3 V"); + Assert.AreEqual(18, r.depth, "Scale 4 5 6 , 1 2 3 D"); + + v2 = new(-1, -2, -3); + r = Vector3.Scale(v1, v2); + Assert.AreEqual(-4, r.horizontal, "Scale 4 5 6 , -1 -2 -3 H"); + Assert.AreEqual(-10, r.vertical, "Scale 4 5 6 , -1 -2 -3 V"); + Assert.AreEqual(-18, r.depth, "Scale 4 5 6 , -1 -2 -3 D"); + + v2 = new(0, 0, 0); + r = Vector3.Scale(v1, v2); + Assert.AreEqual(0, r.horizontal, "Scale 4 5 6 , 0 0 0 H"); + Assert.AreEqual(0, r.vertical, "Scale 4 5 6 , 0 0 0 V"); + Assert.AreEqual(0, r.depth, "Scale 4 5 6 , 0 0 0 D"); + } + + [Test] + public void Multiply() { + Vector3 v1 = new(4, 5, 6); + int f = 3; + Vector3 r; + + r = v1 * f; + Assert.AreEqual(12, r.horizontal, "4 5 6 * 3 H"); + Assert.AreEqual(15, r.vertical, "4 5 6 * 3 V"); + Assert.AreEqual(18, r.depth, "4 5 6 * 3 D"); + + r = f * v1; + Assert.AreEqual(12, r.horizontal, "3 * 4 5 6 H"); + Assert.AreEqual(15, r.vertical, "3 * 4 5 6 V"); + Assert.AreEqual(18, r.depth, "3 * 4 5 6 D"); + + f = -3; + r = v1 * f; + Assert.AreEqual(-12, r.horizontal, "4 5 6 * -3 H"); + Assert.AreEqual(-15, r.vertical, "4 5 6 * -3 V"); + Assert.AreEqual(-18, r.depth, "4 5 6 * -3 D"); + + f = 0; + r = v1 * f; + Assert.AreEqual(0, r.horizontal, "4 5 6 * 0 H"); + Assert.AreEqual(0, r.vertical, "4 5 6 * 0 V"); + Assert.AreEqual(0, r.depth, "4 5 6 * 0 D"); + } + + [Test] + public void Divide() { + Vector3 v1 = new(4, 5, 6); + int f = 2; + Vector3 r; + + r = v1 / f; + Assert.AreEqual(2, r.horizontal, "4 5 6 / 2 H"); + Assert.AreEqual(2, r.vertical, "4 5 6 / 2 V"); + Assert.AreEqual(3, r.depth, "4 5 6 / 2 D"); + + f = -2; + r = v1 / f; + Assert.AreEqual(-2, r.horizontal, "4 5 6 / -2 H"); + Assert.AreEqual(-2, r.vertical, "4 5 6 / -2 V"); + Assert.AreEqual(-3, r.depth, "4 5 6 / -2 D"); + + Assert.Throws(() => { + f = 0; + r = v1 / f; + }); + } + + [Test] + public void Dot() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + float f; + + f = Vector3.Dot(v1, v2); + Assert.AreEqual(32, f, "Dot(4 5 6, 1 2 3)"); + + v2 = new(-1, -2, -3); + f = Vector3.Dot(v1, v2); + Assert.AreEqual(-32, f, "Dot(4 5 6, -1 -2 -3)"); + + v2 = new(0, 0, 0); + f = Vector3.Dot(v1, v2); + Assert.AreEqual(0, f, "Dot(4 5 6, 0 0 0)"); + } + + [Test] + public void Cross() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 r; + + r = Vector3.Cross(v1, v2); + Assert.AreEqual(3, r.horizontal, "Cross(4 5 6, 1 2 3) H"); + Assert.AreEqual(-6, r.vertical, "Cross(4 5 6, 1 2 3) V"); + Assert.AreEqual(3, r.depth, "Cross(4 5 6, 1 2 3) D"); + + v2 = new(-1, -2, -3); + r = Vector3.Cross(v1, v2); + Assert.AreEqual(-3, r.horizontal, "Cross(4 5 6, -1 -2 -3) H"); + Assert.AreEqual(6, r.vertical, "Cross(4 5 6, -1 -2 -3) V"); + Assert.AreEqual(-3, r.depth, "Cross(4 5 6, -1 -2 -3) D"); + + v2 = new(0, 0, 0); + r = Vector3.Cross(v1, v2); + Assert.AreEqual(0, r.horizontal, "Cross(4 5 6, 0 0 0) H"); + Assert.AreEqual(0, r.vertical, "Cross(4 5 6, 0 0 0) V"); + Assert.AreEqual(0, r.depth, "Cross(4 5 6, 0 0 0) D"); + } + + [Test] + public void UnsignedAngle() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + AngleFloat a; + + a = Vector3.UnsignedAngle(v1, v2); + Assert.AreEqual(12.9331379F, a.inDegrees, "Angle(4 5 6, 1 2 3)"); + + v2 = new(-1, -2, -3); + a = Vector3.UnsignedAngle(v1, v2); + Assert.AreEqual(167.066849F, a.inDegrees, "Angle(4 5 6, -1 -2 -3)"); + + v2 = new(0, 0, 0); + a = Vector3.UnsignedAngle(v1, v2); + Assert.AreEqual(0, a.inDegrees, "Angle(4 5 6, 0 0 0)"); + } + + [Test] + public void SignedAngle() { + Vector3 v1 = new(4, 5, 6); + Vector3 v2 = new(1, 2, 3); + Vector3 v3 = new(7, 8, -9); + AngleFloat a; + + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(-12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 7 8 -9)"); + + v2 = new(-1, -2, -3); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(167.066849F, a.inDegrees, "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)"); + + v2 = new(0, 0, 0); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 0 0 0, 7 8 -9)"); + + v2 = new(1, 2, 3); + v3 = new(-7, -8, 9); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(12.9331379F, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, -7 -8 9)"); + + v3 = new(0, 0, 0); + a = Vector3.SignedAngle(v1, v2, v3); + Assert.AreEqual(0, a.inDegrees, "SignedAngle(4 5 6, 1 2 3, 0 0 0)"); + } + } +} +#endif \ No newline at end of file diff --git a/MemoryCell.cs b/MemoryCell.cs new file mode 100644 index 0000000..6e20a75 --- /dev/null +++ b/MemoryCell.cs @@ -0,0 +1,65 @@ +using System; +using UnityEngine; +using Unity.Mathematics; +using static Unity.Mathematics.math; + +[Serializable] +public class MemoryCell : Neuron { + + public MemoryCell(ClusterPrefab cluster, string name) : base(cluster, name) {} + + #region Parameters + + // Returns the memorized value weighted by time + // return lastValue * (current time - last time) + [SerializeField] + public bool deltaValue = false; + + #endregion Parameters + + #region State + + private float3 _memorizedValue; + private float _memorizedTime; + + public override void UpdateState() { + // A memorycell does not have an activation function + float3 result = new(0, 0, 0); + int n = 0; + + //Applying the weight factgors + foreach (Synapse synapse in this.synapses) { + if (synapse.nucleus == this) { + float deltaTime = Time.time - this.lastTime; + synapse.weight = deltaTime; + } + result += synapse.weight * synapse.nucleus.outputValue; + if (lengthsq(synapse.nucleus.outputValue) != 0) + n++; + } + + if (this.average) + result /= n; + + UpdateResult(result); + } + + public override void UpdateResult(Vector3 result) { + // output value is the previous value + if (this.deltaValue) { + float deltaTime = Time.time - this._memorizedTime; + this._outputValue = this._memorizedValue * deltaTime; + } + else + this._outputValue = this._memorizedValue; + + // Store the result for the next time + this._memorizedValue = result; + this._memorizedTime = Time.time; + + foreach (INucleus receiver in this.receivers) + receiver.UpdateState(); + } + + #endregion State +} diff --git a/MemoryCell.cs.meta b/MemoryCell.cs.meta new file mode 100644 index 0000000..ef74aba --- /dev/null +++ b/MemoryCell.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 29633aa3fe5cd9dcc8d886051f45d4d8 \ No newline at end of file diff --git a/NanoBrain-Unity.code-workspace b/NanoBrain-Unity.code-workspace new file mode 100644 index 0000000..5194438 --- /dev/null +++ b/NanoBrain-Unity.code-workspace @@ -0,0 +1,12 @@ +{ + "folders": [ + { + "path": "../.." + }, + { + "name": "LinearAlgebra-csharp", + "path": "LinearAlgebra-csharp" + } + ], + "settings": {} +} \ No newline at end of file diff --git a/NanoBrain-Unity.code-workspace.meta b/NanoBrain-Unity.code-workspace.meta new file mode 100644 index 0000000..65bb132 --- /dev/null +++ b/NanoBrain-Unity.code-workspace.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: cfec45da5945b94d684a763d86b0dcf8 +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Neuroid.cs b/Neuroid.cs new file mode 100644 index 0000000..d4a64f2 --- /dev/null +++ b/Neuroid.cs @@ -0,0 +1,82 @@ +/* +using UnityEngine; +using Unity.Mathematics; +using static Unity.Mathematics.math; + +[System.Serializable] +public class Neuroid : Neuron { + + public bool average = false; + + public Neuroid(Cluster brain, string name) : base(name) { + this.cluster = brain; + if (this.cluster != null) { + this.cluster.nuclei.Add(this); + } + else + Debug.LogError("No neuroid network"); + } + + public Neuroid(string name) : base(name) { } + + public override INucleus Clone() { + Neuroid clone = new(this.name) { + cluster = this.cluster, + array = this.array, + curve = this.curve, + curvePreset = this.curvePreset, + curveMax = this.curveMax, + average = this.average + }; + if (clone.cluster != null) + clone.cluster.nuclei.Add(clone); + + foreach (Synapse synapse in this.synapses) { + Synapse clonedSynapse = clone.AddSynapse(synapse.nucleus); + clonedSynapse.weight = synapse.weight; + } + foreach (INucleus receiver in this.receivers) { + clone.AddReceiver(receiver); + } + return clone; + } + + public override void UpdateState() { + float3 sum = new(0, 0, 0); + int n = 0; + + //Applying the weight factgors + foreach (Synapse synapse in this.synapses) { + sum = sum + (synapse.weight * synapse.nucleus.outputValue); + if (lengthsq(synapse.nucleus.outputValue) != 0) + n++; + } + if (average) + sum /= n; + + // Activation function + Vector3 result; + switch (this.curvePreset) { + case CurvePresets.Linear: + result = sum; + break; + case CurvePresets.Sqrt: + result = normalize(sum) * System.MathF.Sqrt(length(sum)); + break; + case CurvePresets.Power: + result = normalize(sum) * System.MathF.Pow(length(sum), 2); + break; + case CurvePresets.Reciprocal: + result = normalize(sum) * (1 / length(sum)); + break; + default: + float activatedValue = this.curve.Evaluate(length(sum)); + result = normalize(sum) * activatedValue; + break; + } + UpdateResult(result); + } + +} + +*/ \ No newline at end of file diff --git a/Neuroid.cs.meta b/Neuroid.cs.meta new file mode 100644 index 0000000..1c633f0 --- /dev/null +++ b/Neuroid.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 771f64aec709af240a39b1d918bbc829 \ No newline at end of file diff --git a/Neuron.cs b/Neuron.cs new file mode 100644 index 0000000..099ceab --- /dev/null +++ b/Neuron.cs @@ -0,0 +1,321 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using UnityEngine; +using UnityEditor; +using Unity.Mathematics; +using static Unity.Mathematics.math; + +[Serializable] +public class Neuron : INucleus { + + [SerializeField] + protected string _name; + public virtual string name { + get => _name; + set => _name = value; + } + + [SerializeField] + private List _synapses = new(); + public List synapses => _synapses; + + [SerializeReference] + private List _receivers = new(); + public List receivers { + get { return _receivers; } + set { _receivers = value; } + } + + [SerializeReference] + private NucleusArray _array; + public NucleusArray array { + get { return _array; } + set { _array = value; } + } + + #region Serialization + + public enum CurvePresets { + Linear, + Power, + Sqrt, + Reciprocal, + Custom + } + [SerializeField] + private CurvePresets _curvePreset; + public CurvePresets curvePreset { + get { return _curvePreset; } + set { + _curvePreset = value; + this.curve = GenerateCurve(); + } + } + public AnimationCurve curve; + public float curveMax = 1.0f; + + #region Parameters + + public bool average = false; + + #endregion Parameters + + public AnimationCurve GenerateCurve() { + switch (this.curvePreset) { + case CurvePresets.Linear: + this.curveMax = 1; + return Presets.Linear(1); + case CurvePresets.Power: + this.curveMax = 1; + return Presets.Power(2.0f, 1); + case CurvePresets.Sqrt: + this.curveMax = 1; + return Presets.Power(0.5f, 1); + case CurvePresets.Reciprocal: + this.curveMax = 1 / 0.01f * 1; + return Presets.Reciprocal(1); + default: + this.curveMax = 1; + return this.curve; + } + } + + public virtual void Deserialize(Neuron nucleus) { } + + #endregion Serialization + + #region Runtime state (not serialized) + + public ClusterPrefab cluster { get; set; } + + #region Activation + + public static class Presets { + private const int samples = 32; + public static AnimationCurve Linear(float weight) { + return AnimationCurve.Linear(0f, 0f, 1000f, weight * 1000); + } + public static AnimationCurve Power(float exponent, float weight) { + // build keyframes + Keyframe[] keys = new Keyframe[samples]; + for (int i = 0; i < samples; i++) { + float t = i / (float)(samples - 1); + float v = Mathf.Pow(t, exponent) * weight; + keys[i] = new Keyframe(t, v); + } + + AnimationCurve curve = new(keys); + + // set tangent modes for each key to Auto (smooth). Use Linear if you prefer straight segments. + for (int i = 0; i < curve.length; i++) { + AnimationUtility.SetKeyLeftTangentMode(curve, i, AnimationUtility.TangentMode.Auto); + AnimationUtility.SetKeyRightTangentMode(curve, i, AnimationUtility.TangentMode.Auto); + } + + return curve; + } + public static AnimationCurve Reciprocal(float weight) { + int samples = 128; + float xMin = 0.001f; + float xMax = 1; + var keys = new Keyframe[samples]; + for (int i = 0; i < samples; i++) { + float t = i / (float)(samples - 1); + float x = Mathf.Lerp(xMin, xMax, t); + float y = 1f / x * weight; + keys[i] = new Keyframe(x, y); + } + var curve = new AnimationCurve(keys); + for (int i = 0; i < curve.length; i++) { + AnimationUtility.SetKeyLeftTangentMode(curve, i, AnimationUtility.TangentMode.Linear); + AnimationUtility.SetKeyRightTangentMode(curve, i, AnimationUtility.TangentMode.Linear); + } + return curve; + } + } + + #endregion Activation + + protected float3 _outputValue; + public virtual float3 outputValue { + get { return _outputValue; } + set { + this.stale = 0; + // this._isSleeping = false; + _outputValue = value; + } + } + + [NonSerialized] + private int stale = 1000; + + // private bool _isSleeping = false; + // public bool isSleeping => _isSleeping; + public bool isSleeping => lengthsq(this.outputValue) == 0; + public float lastTime { get; private set; } + + public void UpdateNuclei() { + this.stale++; + // this._isSleeping = this.stale > 2; + // if (isSleeping) + if (this.stale > 2) + _outputValue = Vector3.zero; + } + + #endregion Runtime state + + public Neuron(ClusterPrefab parent, string name) { + this.cluster = parent; + this.name = name; + if (this.cluster != null) { + this.cluster.nuclei.Add(this); + } + // else + // Debug.LogError("No neuroid network"); + } + + // public Neuron(string name) { + // this._name = name; + // } + + public virtual IReceptor CloneTo(ClusterPrefab parent) { + Neuron clone = new(parent, this.name) { + array = this.array, + curve = this.curve, + curvePreset = this.curvePreset, + curveMax = this.curveMax, + average = this.average + }; + // if (clone.cluster != null) + // clone.cluster.nuclei.Add(clone); + + foreach (Synapse synapse in this.synapses) { + Synapse clonedSynapse = clone.AddSynapse(synapse.nucleus); + clonedSynapse.weight = synapse.weight; + } + foreach (INucleus receiver in this.receivers) { + clone.AddReceiver(receiver); + } + return clone; + } + public virtual IReceptor Clone() { + Neuron clone = new(this.cluster, this.name) { + array = this.array, + curve = this.curve, + curvePreset = this.curvePreset, + curveMax = this.curveMax, + average = this.average + }; + // if (clone.cluster != null) + // clone.cluster.nuclei.Add(clone); + + foreach (Synapse synapse in this.synapses) { + Synapse clonedSynapse = clone.AddSynapse(synapse.nucleus); + clonedSynapse.weight = synapse.weight; + } + foreach (INucleus receiver in this.receivers) { + clone.AddReceiver(receiver); + } + return clone; + } + + public virtual void AddReceiver(INucleus receivingNucleus) { + this._receivers.Add(receivingNucleus); + receivingNucleus.AddSynapse(this); + } + + public void RemoveReceiver(INucleus receiverNucleus) { + this._receivers.RemoveAll(receiver => receiver == receiverNucleus); + receiverNucleus.synapses.RemoveAll(synapse => synapse.nucleus == this); + } + + public static void Delete(INucleus nucleus) { + foreach (Synapse synapse in nucleus.synapses) { + if (synapse.nucleus is Neuron synapse_nucleus) { + if (synapse_nucleus._receivers.Count > 1) { + // there is another nucleus feeding into this input nucleus + synapse_nucleus._receivers.RemoveAll(r => r == nucleus); + } + else { + // No other links, delete it. + Neuron.Delete(synapse_nucleus); + } + } + } + foreach (INucleus receiver in nucleus.receivers) { + if (receiver != null && receiver.synapses != null) + receiver.synapses.RemoveAll(s => s.nucleus == nucleus); + } + + if (nucleus.cluster != null) { + nucleus.cluster.nuclei.RemoveAll(n => n == nucleus); + nucleus.cluster.GarbageCollection(); + } + } + + public Synapse AddSynapse(IReceptor sendingNucleus) { + Synapse synapse = new(sendingNucleus); + this.synapses.Add(synapse); + return synapse; + } + + public virtual void UpdateState() { + UpdateState(new float3(0, 0, 0)); + } + + public virtual void UpdateState(float3 inputValue) { + float3 sum = inputValue;//new(0, 0, 0); + int n = 0; + + //Applying the weight factgors + foreach (Synapse synapse in this.synapses) { + if (synapse.nucleus == this) { + float deltaTime = Time.time - this.lastTime; + synapse.weight = deltaTime; + } + sum += synapse.weight * synapse.nucleus.outputValue; + // Perhaps synapses should be removed when the output value goes to 0.... + if (lengthsq(synapse.nucleus.outputValue) != 0) + n++; + } + if (this.average && n > 0) + sum /= n; + + // Activation function + Vector3 result; + switch (this.curvePreset) { + case CurvePresets.Linear: + result = sum; + break; + case CurvePresets.Sqrt: + result = normalize(sum) * System.MathF.Sqrt(length(sum)); + break; + case CurvePresets.Power: + result = normalize(sum) * System.MathF.Pow(length(sum), 2); + break; + case CurvePresets.Reciprocal: + result = normalize(sum) * (1 / length(sum)); + break; + default: + float activatedValue = this.curve.Evaluate(length(sum)); + result = normalize(sum) * activatedValue; + break; + } + UpdateResult(result); + } + + public virtual void UpdateResult(Vector3 result) { + // float d = Vector3.Distance(result, this.outputValue); + // if (d < 0.5f) { + // //Debug.Log($"insignificant update: {d}"); + // return; + // } + + this.outputValue = result; + this.lastTime = Time.time; + foreach (INucleus receiver in this.receivers) + receiver.UpdateState(); + + } +} \ No newline at end of file diff --git a/Neuron.cs.meta b/Neuron.cs.meta new file mode 100644 index 0000000..e520090 --- /dev/null +++ b/Neuron.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 750748f3f0e7d472fbf88ab02987074c \ No newline at end of file diff --git a/NucleusArray.cs b/NucleusArray.cs new file mode 100644 index 0000000..82a3b0e --- /dev/null +++ b/NucleusArray.cs @@ -0,0 +1,67 @@ +using System.Linq; +using System.Collections.Generic; +using UnityEngine; + +[System.Serializable] +public class NucleusArray { + [SerializeReference] + private INucleus[] _nuclei; + private ClusterPrefab[] _clusters; + public IEnumerable nuclei { + get { + // if (_nuclei == null) + // return _clusters; + // else if (_clusters == null) + return _nuclei; + // else + // return _nuclei.Concat(_clusters); + } + } + public string name; + + public NucleusArray(INucleus nucleus) { + this.name = nucleus.name; + this._nuclei = new INucleus[1]; + this._nuclei[0] = nucleus; + this._clusters = new ClusterPrefab[0]; + } + public NucleusArray(ClusterPrefab cluster) { + this.name = cluster.name; + this._nuclei = new INucleus[0]; + this._clusters = new ClusterPrefab[1]; + this._clusters[0] = cluster; + } + + public void AddNucleus() { + if (this._nuclei.Length == 0) { + Debug.LogError("Empty perceptoid array, cannot add"); + return; + } + int newLength = this._nuclei.Length + 1; + INucleus[] newArray = new INucleus[newLength]; + + for (int i = 0; i < this._nuclei.Length; i++) + newArray[i] = this._nuclei[i]; + if (this._nuclei[0] is INucleus nucleus) + newArray[newLength - 1] = (INucleus) nucleus.Clone(); + + this._nuclei = newArray; + } + + public void RemoveNucleus() { + int newLength = this._nuclei.Length - 1; + if (newLength == 0) { + Debug.LogWarning("Perceptoid array cannot be empty"); + return; + } + INucleus[] newPerceptei = new INucleus[newLength]; + for (int i = 0; i < newLength; i++) + newPerceptei[i] = this._nuclei[i]; + // Delete the last perception + Neuron.Delete(this._nuclei[newLength]); + + this._nuclei = newPerceptei; + } + + +} \ No newline at end of file diff --git a/NucleusArray.cs.meta b/NucleusArray.cs.meta new file mode 100644 index 0000000..61e26b7 --- /dev/null +++ b/NucleusArray.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: f8cac60bd79854595a8571c042f77998 \ No newline at end of file diff --git a/Perceptoid.cs b/Perceptoid.cs new file mode 100644 index 0000000..447a3d7 --- /dev/null +++ b/Perceptoid.cs @@ -0,0 +1,105 @@ +/* +using UnityEngine; + +[System.Serializable] +public class Perceptoid : Neuroid { + // A neuroid which has no neurons as input + // But receives value from a receptor + + public NanoBrain brain; + public Receptor receptor; + public string baseName; + + public int thingId; + + //[SerializeField] + // Needs serialization!!!! + [SerializeReference] + public PercepteiArray array; + + #region Serialization + + [SerializeField] + public int thingType; + + public override void Rebuild(NanoBrain brain) { + base.Rebuild(brain); + this.receptor = Receptor.GetReceptor(brain, thingType); + this.receptor.perceptei.Add(this); + if (string.IsNullOrEmpty(this.baseName)) + this.baseName = this.name; + } + + public override void Deserialize(Nucleus nucleus) { + base.Deserialize(nucleus); + + if (nucleus is Perceptoid perceptoid) + this.receptor.thingType = perceptoid.thingType; + + // Point all receivers to this perceptoid instead of the default nucleus + foreach (INucleus receiver in nucleus.receivers) { + foreach (Synapse synapse in receiver.synapses) { + if (synapse.nucleus == nucleus) + synapse.nucleus = this; + } + } + // Point all synapses to this perceptoid instead of the default nucleus + // foreach (Synapse synapse in nucleus.synapses) { + // foreach (INucleus r in synapse.nucleus.receivers) { + // if (r == nucleus) + // this.receiver = this; + // } + // } + // Copying disabled for now + // // Copy all the synapses + // this.synapses = nucleus.synapses; + // // Copy all receivers + // this.receivers = nucleus.receivers; + } + + #endregion Serialization + + public Perceptoid(NanoBrain brain, int thingType, string name = "sensor") : base(name) { + this.brain = brain; + this.cluster = brain.cluster; + if (this.cluster != null) { + brain.perceptei.Add(this); + } + else + Debug.LogError("No neuroid network"); + + this.nucleusType = nameof(Perceptoid); + this.name = name; + this.baseName = name; + this.thingType = thingType; + this.receptor = Receptor.GetReceptor(brain, thingType); + this.receptor.perceptei.Add(this); + this.array = new PercepteiArray(this); + } + + public Perceptoid(PercepteiArray array) : base(array.name) { + this.array = array; + Perceptoid source = array.perceptei[0]; + this.brain = source.brain; + this.cluster = source.cluster; + if (this.brain != null) { + this.brain.perceptei.Add(this); + } + else + Debug.LogError("No neuroid network"); + + this.nucleusType = nameof(Perceptoid); + this.name = source.baseName; + this.baseName = source.baseName; + this.thingType = source.thingType; + this.receptor = Receptor.GetReceptor(this.brain, this.thingType); + this.receptor.perceptei.Add(this); + } + + public override void UpdateState() { + Vector3 result = this.receptor.localPosition; + UpdateResult(result); + } + +} +*/ \ No newline at end of file diff --git a/Perceptoid.cs.meta b/Perceptoid.cs.meta new file mode 100644 index 0000000..ebac122 --- /dev/null +++ b/Perceptoid.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 702f634001a21a9d7ae1057c8ce356e9 \ No newline at end of file diff --git a/Receptor.cs b/Receptor.cs new file mode 100644 index 0000000..72ef7df --- /dev/null +++ b/Receptor.cs @@ -0,0 +1,175 @@ +using System.Collections.Generic; +using UnityEngine; +using Unity.Mathematics; +using static Unity.Mathematics.math; + +public class Receptor : IReceptor { + + private ClusterPrefab cluster; + + [SerializeField] + protected string _name; + public virtual string name { + get => _name; + set => _name = value; + } + + public Receptor(ClusterPrefab cluster) { + this.cluster = cluster; + if (cluster != null) + cluster.nuclei.Add(this); + } + + public Receptor(ClusterPrefab cluster, INucleus nucleus) { + this.cluster = cluster; + if (cluster != null) + cluster.nuclei.Add(this); + this.AddReceiver(nucleus); + } + + public static Receptor CreateReceptor(ClusterPrefab cluster, string nucleusName) { + if (cluster == null) + return null; + + Receptor receptor = new(cluster); + foreach (INucleus nucleus in cluster.inputs) { + if (nucleus != null && nucleus.name == nucleusName) { + // Receptor receptor = new(cluster, nucleus); + // return receptor; + receptor.AddReceiver(nucleus); + } + } + if (receptor._receivers.Count == 0) + return null; + else + return receptor; + } + + public virtual IReceptor CloneTo(ClusterPrefab parent) { + Receptor clone = new(parent); + + foreach (INucleus receiver in this.receivers) { + clone.AddReceiver(receiver); + } + + return clone; + } + public virtual IReceptor Clone() { + Receptor clone = new(this.cluster); + + foreach (INucleus receiver in this.receivers) { + clone.AddReceiver(receiver); + } + + return clone; + } + + class Receiver { + public INucleus nucleus; + public int thingId; + public string thingName; + public Receiver(INucleus nucleus, int thingId, string thingName) { + this.nucleus = nucleus; + this.thingId = thingId; + this.thingName = thingName; + } + } + + [SerializeReference] + private List _receivers = new(); + public List receivers { + get { return _receivers; } + set { _receivers = value; } + } + + protected int[] thingIds; // every receiver can handle a thing with this id + + public virtual void AddReceiver(INucleus receivingNucleus) { + this._receivers.Add(receivingNucleus); + receivingNucleus.AddSynapse(this); + } + + public void RemoveReceiver(INucleus receiverNucleus) { + this._receivers.RemoveAll(receiver => receiver == receiverNucleus); + receiverNucleus.synapses.RemoveAll(synapse => synapse.nucleus == this); + } + + private int stale = 1000; + + private bool _isSleeping = false; + public bool isSleeping => _isSleeping; + + public Vector3 localPosition { + set { + this.stale = 0; + this._isSleeping = false; + this._outputValue = value; + + } + } + public float distanceResolution = 0.1f; + public float directionResolution = 5; + + private float3 _outputValue; + public float3 outputValue { + get { return this._outputValue; } + set { + this.stale = 0; + this._isSleeping = false; + this._outputValue = value; + } + } + + public virtual void ProcessStimulus(int thingId, Vector3 newLocalPositionVector, string thingName = null) { + this.localPosition = newLocalPositionVector; + + thingIds ??= new int[this._receivers.Count]; + + int receiverIx = 0; + INucleus selectedReceiver = null; + int selectedReceiverIx = 0; + foreach (INucleus receiver in this.receivers) { + if (thingIds[receiverIx] == thingId) { + // We found an existing receiver for this thing + selectedReceiver = receiver; + selectedReceiverIx = receiverIx; + // Do not look any further + break; + } + else if (receiver.isSleeping) { + // A sleeping receiver is not active and can therefore always be used + selectedReceiver = receiver; + selectedReceiverIx = receiverIx; + // Look further because we may find an existing receiver for this thing + } + else if (selectedReceiver == null) { + // If we haven't found a receiver yet, just start by taking the first + selectedReceiver = receiver; + selectedReceiverIx = receiverIx; + } + else if (selectedReceiver.isSleeping == false) { + // If no existing or sleeping receiver is found, we look for + // the receiver with the furthest/least interesting stimulus + if (length(receiver.outputValue) < length(selectedReceiver.outputValue)) { + // Debug.Log($"{selectedReceiver.name}[{selectedReceiverIx}] {length(selectedReceiver.outputValue)}" + + // $" {receiver.name}[{receiverIx}] {length(receiver.outputValue)} "); + selectedReceiver = receiver; + selectedReceiverIx = receiverIx; + } + } + receiverIx++; + } + // Debug.Log($"Receiver {selectedReceiver.name}[{selectedReceiverIx}] for thing {thingId}"); + thingIds[selectedReceiverIx] = thingId; + // if (thingName != null) + // selectedReceiver.nucleus.name = selectedReceiver.nucleus.baseName + " " + thingName; + selectedReceiver.UpdateState(); + } + + public void UpdateNuclei() { + this.stale++; + this._isSleeping = this.stale > 2; + if (isSleeping) + this._outputValue = Vector3.zero; + } +} \ No newline at end of file diff --git a/Receptor.cs.meta b/Receptor.cs.meta new file mode 100644 index 0000000..56793ae --- /dev/null +++ b/Receptor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: cfb9734aebc3ab85aacf87d26fb92e55 \ No newline at end of file diff --git a/Scene.meta b/Scene.meta new file mode 100644 index 0000000..d71b5e5 --- /dev/null +++ b/Scene.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: bfd7dadd61c0891d8a94db0196e61a8a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Scene/TestScene.unity b/Scene/TestScene.unity new file mode 100644 index 0000000..401756e --- /dev/null +++ b/Scene/TestScene.unity @@ -0,0 +1,487 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!29 &1 +OcclusionCullingSettings: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_OcclusionBakeSettings: + smallestOccluder: 5 + smallestHole: 0.25 + backfaceThreshold: 100 + m_SceneGUID: 00000000000000000000000000000000 + m_OcclusionCullingData: {fileID: 0} +--- !u!104 &2 +RenderSettings: + m_ObjectHideFlags: 0 + serializedVersion: 10 + m_Fog: 0 + m_FogColor: {r: 0.5, g: 0.5, b: 0.5, a: 1} + m_FogMode: 3 + m_FogDensity: 0.01 + m_LinearFogStart: 0 + m_LinearFogEnd: 300 + m_AmbientSkyColor: {r: 0.212, g: 0.227, b: 0.259, a: 1} + m_AmbientEquatorColor: {r: 0.114, g: 0.125, b: 0.133, a: 1} + m_AmbientGroundColor: {r: 0.047, g: 0.043, b: 0.035, a: 1} + m_AmbientIntensity: 1 + m_AmbientMode: 0 + m_SubtractiveShadowColor: {r: 0.42, g: 0.478, b: 0.627, a: 1} + m_SkyboxMaterial: {fileID: 10304, guid: 0000000000000000f000000000000000, type: 0} + m_HaloStrength: 0.5 + m_FlareStrength: 1 + m_FlareFadeSpeed: 3 + m_HaloTexture: {fileID: 0} + m_SpotCookie: {fileID: 10001, guid: 0000000000000000e000000000000000, type: 0} + m_DefaultReflectionMode: 0 + m_DefaultReflectionResolution: 128 + m_ReflectionBounces: 1 + m_ReflectionIntensity: 1 + m_CustomReflection: {fileID: 0} + m_Sun: {fileID: 0} + m_UseRadianceAmbientProbe: 0 +--- !u!157 &3 +LightmapSettings: + m_ObjectHideFlags: 0 + serializedVersion: 13 + m_BakeOnSceneLoad: 0 + m_GISettings: + serializedVersion: 2 + m_BounceScale: 1 + m_IndirectOutputScale: 1 + m_AlbedoBoost: 1 + m_EnvironmentLightingMode: 0 + m_EnableBakedLightmaps: 1 + m_EnableRealtimeLightmaps: 0 + m_LightmapEditorSettings: + serializedVersion: 12 + m_Resolution: 2 + m_BakeResolution: 40 + m_AtlasSize: 1024 + m_AO: 0 + m_AOMaxDistance: 1 + m_CompAOExponent: 1 + m_CompAOExponentDirect: 0 + m_ExtractAmbientOcclusion: 0 + m_Padding: 2 + m_LightmapParameters: {fileID: 0} + m_LightmapsBakeMode: 1 + m_TextureCompression: 1 + m_ReflectionCompression: 2 + m_MixedBakeMode: 2 + m_BakeBackend: 2 + m_PVRSampling: 1 + m_PVRDirectSampleCount: 32 + m_PVRSampleCount: 512 + m_PVRBounces: 2 + m_PVREnvironmentSampleCount: 256 + m_PVREnvironmentReferencePointCount: 2048 + m_PVRFilteringMode: 1 + m_PVRDenoiserTypeDirect: 1 + m_PVRDenoiserTypeIndirect: 1 + m_PVRDenoiserTypeAO: 1 + m_PVRFilterTypeDirect: 0 + m_PVRFilterTypeIndirect: 0 + m_PVRFilterTypeAO: 0 + m_PVREnvironmentMIS: 1 + m_PVRCulling: 1 + m_PVRFilteringGaussRadiusDirect: 1 + m_PVRFilteringGaussRadiusIndirect: 1 + m_PVRFilteringGaussRadiusAO: 1 + m_PVRFilteringAtrousPositionSigmaDirect: 0.5 + m_PVRFilteringAtrousPositionSigmaIndirect: 2 + m_PVRFilteringAtrousPositionSigmaAO: 1 + m_ExportTrainingData: 0 + m_TrainingDataDestination: TrainingData + m_LightProbeSampleCountMultiplier: 4 + m_LightingDataAsset: {fileID: 20201, guid: 0000000000000000f000000000000000, type: 0} + m_LightingSettings: {fileID: 0} +--- !u!196 &4 +NavMeshSettings: + serializedVersion: 2 + m_ObjectHideFlags: 0 + m_BuildSettings: + serializedVersion: 3 + agentTypeID: 0 + agentRadius: 0.5 + agentHeight: 2 + agentSlope: 45 + agentClimb: 0.4 + ledgeDropHeight: 0 + maxJumpAcrossDistance: 0 + minRegionArea: 2 + manualCellSize: 0 + cellSize: 0.16666667 + manualTileSize: 0 + tileSize: 256 + buildHeightMesh: 0 + maxJobWorkers: 0 + preserveTilesOutsideBounds: 0 + debug: + m_Flags: 0 + m_NavMeshData: {fileID: 0} +--- !u!1001 &551770709 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + serializedVersion: 3 + m_TransformParent: {fileID: 0} + m_Modifications: + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalPosition.x + value: 0.71 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalPosition.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093763, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_Name + value: Boid2 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_RemovedGameObjects: [] + m_AddedGameObjects: [] + m_AddedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} +--- !u!1 &968074744 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 968074747} + - component: {fileID: 968074746} + - component: {fileID: 968074745} + m_Layer: 0 + m_Name: Main Camera + m_TagString: MainCamera + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!81 &968074745 +AudioListener: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 968074744} + m_Enabled: 1 +--- !u!20 &968074746 +Camera: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 968074744} + m_Enabled: 1 + serializedVersion: 2 + m_ClearFlags: 1 + m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0} + m_projectionMatrixMode: 1 + m_GateFitMode: 2 + m_FOVAxisMode: 0 + m_Iso: 200 + m_ShutterSpeed: 0.005 + m_Aperture: 16 + m_FocusDistance: 10 + m_FocalLength: 50 + m_BladeCount: 5 + m_Curvature: {x: 2, y: 11} + m_BarrelClipping: 0.25 + m_Anamorphism: 0 + m_SensorSize: {x: 36, y: 24} + m_LensShift: {x: 0, y: 0} + m_NormalizedViewPortRect: + serializedVersion: 2 + x: 0 + y: 0 + width: 1 + height: 1 + near clip plane: 0.3 + far clip plane: 1000 + field of view: 60 + orthographic: 0 + orthographic size: 5 + m_Depth: -1 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingPath: -1 + m_TargetTexture: {fileID: 0} + m_TargetDisplay: 0 + m_TargetEye: 3 + m_HDR: 1 + m_AllowMSAA: 1 + m_AllowDynamicResolution: 0 + m_ForceIntoRT: 0 + m_OcclusionCulling: 1 + m_StereoConvergence: 10 + m_StereoSeparation: 0.022 +--- !u!4 &968074747 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 968074744} + serializedVersion: 2 + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 1, z: -10} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 0} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &1342149740 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 1342149742} + - component: {fileID: 1342149741} + m_Layer: 0 + m_Name: SwamControl + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!114 &1342149741 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1342149740} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 0464906885ae3494f8fd0314719fb2db, type: 3} + m_Name: + m_EditorClassIdentifier: Assembly-CSharp::SwarmControl + speed: 0.5 + inertia: 0.1 + alignmentForce: 0 + cohesionForce: 1 + separationForce: 1 + avoidanceForce: 5 + separationDistance: 0.5 + perceptionDistance: 1 + spaceSize: {x: 10, y: 10, z: 10} + boundaryWidth: {x: 1, y: 1, z: 1} +--- !u!4 &1342149742 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 1342149740} + serializedVersion: 2 + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: -1.00377, y: -1.02283, z: 0.72231} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 0} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &2011285159 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2011285161} + - component: {fileID: 2011285160} + m_Layer: 0 + m_Name: Directional Light + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!108 &2011285160 +Light: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2011285159} + m_Enabled: 1 + serializedVersion: 12 + m_Type: 1 + m_Color: {r: 1, g: 0.95686275, b: 0.8392157, a: 1} + m_Intensity: 1 + m_Range: 10 + m_SpotAngle: 30 + m_InnerSpotAngle: 21.80208 + m_CookieSize2D: {x: 0.5, y: 0.5} + m_Shadows: + m_Type: 2 + m_Resolution: -1 + m_CustomResolution: -1 + m_Strength: 1 + m_Bias: 0.05 + m_NormalBias: 0.4 + m_NearPlane: 0.2 + m_CullingMatrixOverride: + e00: 1 + e01: 0 + e02: 0 + e03: 0 + e10: 0 + e11: 1 + e12: 0 + e13: 0 + e20: 0 + e21: 0 + e22: 1 + e23: 0 + e30: 0 + e31: 0 + e32: 0 + e33: 1 + m_UseCullingMatrixOverride: 0 + m_Cookie: {fileID: 0} + m_DrawHalo: 0 + m_Flare: {fileID: 0} + m_RenderMode: 0 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingLayerMask: 1 + m_Lightmapping: 4 + m_LightShadowCasterMode: 0 + m_AreaSize: {x: 1, y: 1} + m_BounceIntensity: 1 + m_ColorTemperature: 6570 + m_UseColorTemperature: 0 + m_BoundingSphereOverride: {x: 0, y: 0, z: 0, w: 0} + m_UseBoundingSphereOverride: 0 + m_UseViewFrustumForShadowCasterCull: 1 + m_ForceVisible: 0 + m_ShadowRadius: 0 + m_ShadowAngle: 0 + m_LightUnit: 1 + m_LuxAtDistance: 1 + m_EnableSpotReflector: 1 +--- !u!4 &2011285161 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 2011285159} + serializedVersion: 2 + m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261} + m_LocalPosition: {x: 0, y: 3, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 0} + m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0} +--- !u!1001 &4573752827112804207 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + serializedVersion: 3 + m_TransformParent: {fileID: 0} + m_Modifications: + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalPosition.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093762, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 7761516481062093763, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} + propertyPath: m_Name + value: Boid1 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_RemovedGameObjects: [] + m_AddedGameObjects: [] + m_AddedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 6860355b30724b5ddb35781dcaf3b57e, type: 3} +--- !u!1660057539 &9223372036854775807 +SceneRoots: + m_ObjectHideFlags: 0 + m_Roots: + - {fileID: 968074747} + - {fileID: 2011285161} + - {fileID: 4573752827112804207} + - {fileID: 551770709} + - {fileID: 1342149742} diff --git a/Scene/TestScene.unity.meta b/Scene/TestScene.unity.meta new file mode 100644 index 0000000..676153c --- /dev/null +++ b/Scene/TestScene.unity.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 1070383882ed0f5379a3b34e8ccb1f75 +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Synapse.cs b/Synapse.cs new file mode 100644 index 0000000..e7c8116 --- /dev/null +++ b/Synapse.cs @@ -0,0 +1,22 @@ +using System; +using UnityEngine; + +[Serializable] +public class Synapse { + // Support access to cluster of basic nucleus + //public IReceptor nucleus => basicNucleus; + + + //[SerializeReference] + //public Cluster cluster; + + [SerializeReference] + public IReceptor nucleus; + + public float weight; + + public Synapse(IReceptor nucleus, float weight = 1.0f) { + this.nucleus = nucleus; + this.weight = weight; + } +} \ No newline at end of file diff --git a/Synapse.cs.meta b/Synapse.cs.meta new file mode 100644 index 0000000..e62612c --- /dev/null +++ b/Synapse.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 334a58eafccd60cbdb32f719e9e861c6 \ No newline at end of file diff --git a/Velocity.asset b/Velocity.asset new file mode 100644 index 0000000..521ba9b --- /dev/null +++ b/Velocity.asset @@ -0,0 +1,108 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!114 &11400000 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 0} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 60a957541c24c57e78018c202ebb1d9b, type: 3} + m_Name: Velocity + m_EditorClassIdentifier: Assembly-CSharp::Cluster + nuclei: + - rid: 2243601403683012671 + - rid: 2243601403683012676 + references: + version: 2 + RefIds: + - rid: -2 + type: {class: , ns: , asm: } + - rid: 2243601403683012671 + type: {class: Neuron, ns: , asm: Assembly-CSharp} + data: + _name: Output + _synapses: + - nucleus: + rid: -2 + weight: 1 + _receivers: [] + _array: + rid: 2243601403683012672 + _curvePreset: 0 + curve: + serializedVersion: 2 + m_Curve: + - serializedVersion: 3 + time: 0 + value: 0 + inSlope: 0 + outSlope: 1 + tangentMode: 0 + weightedMode: 0 + inWeight: 0 + outWeight: 0 + - serializedVersion: 3 + time: 1000 + value: 1000 + inSlope: 1 + outSlope: 0 + tangentMode: 0 + weightedMode: 0 + inWeight: 0 + outWeight: 0 + m_PreInfinity: 2 + m_PostInfinity: 2 + m_RotationOrder: 4 + curveMax: 1 + average: 0 + - rid: 2243601403683012672 + type: {class: NucleusArray, ns: , asm: Assembly-CSharp} + data: + _nuclei: + - rid: 2243601403683012671 + name: Output + - rid: 2243601403683012676 + type: {class: Neuron, ns: , asm: Assembly-CSharp} + data: + _name: Position + _synapses: [] + _receivers: + - rid: 2243601403683012671 + _array: + rid: 2243601403683012677 + _curvePreset: 0 + curve: + serializedVersion: 2 + m_Curve: + - serializedVersion: 3 + time: 0 + value: 0 + inSlope: 0 + outSlope: 1 + tangentMode: 0 + weightedMode: 0 + inWeight: 0 + outWeight: 0 + - serializedVersion: 3 + time: 1000 + value: 1000 + inSlope: 1 + outSlope: 0 + tangentMode: 0 + weightedMode: 0 + inWeight: 0 + outWeight: 0 + m_PreInfinity: 2 + m_PostInfinity: 2 + m_RotationOrder: 4 + curveMax: 1 + average: 0 + - rid: 2243601403683012677 + type: {class: NucleusArray, ns: , asm: Assembly-CSharp} + data: + _nuclei: + - rid: 2243601403683012676 + name: New neuron diff --git a/Velocity.asset.meta b/Velocity.asset.meta new file mode 100644 index 0000000..07ecb98 --- /dev/null +++ b/Velocity.asset.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: dd622ac7ed09e70ea8edac595047ac82 +NativeFormatImporter: + externalObjects: {} + mainObjectFileID: 11400000 + userData: + assetBundleName: + assetBundleVariant: diff --git a/VisualEditor.meta b/VisualEditor.meta new file mode 100644 index 0000000..d012778 --- /dev/null +++ b/VisualEditor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 62a58c801eda0c9eab7a49fb1d0840cb +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/VisualEditor/Editor.meta b/VisualEditor/Editor.meta new file mode 100644 index 0000000..c068f8e --- /dev/null +++ b/VisualEditor/Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e47ea55fc051fcdcb8ae6197d1105cc0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/VisualEditor/Editor/BrainPickerWindow.cs b/VisualEditor/Editor/BrainPickerWindow.cs new file mode 100644 index 0000000..3a536df --- /dev/null +++ b/VisualEditor/Editor/BrainPickerWindow.cs @@ -0,0 +1,66 @@ +using UnityEditor; +using UnityEngine; +using System; +using System.Linq; + +public class BrainPickerWindow : EditorWindow { + private Vector2 scroll; + private ClusterPrefab[] items = new ClusterPrefab[0]; + private Action onPicked; + private string search = ""; + + public static void ShowPicker(Action onPicked, string title = "Select Cluster") { + var w = CreateInstance(); + w.titleContent = new GUIContent(title); + w.minSize = new Vector2(360, 320); + w.onPicked = onPicked; + w.RefreshList(); + w.ShowModalUtility(); // modal dialog + } + + private void OnEnable() => RefreshList(); + + private void RefreshList() { + var guids = AssetDatabase.FindAssets("t:Cluster"); + items = guids + .Select(g => AssetDatabase.LoadAssetAtPath(AssetDatabase.GUIDToAssetPath(g))) + .Where(b => b != null) + .OrderBy(b => b.name) + .ToArray(); + } + + private void OnGUI() { + EditorGUILayout.Space(); + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField("Choose Cluster:", EditorStyles.boldLabel); + if (GUILayout.Button("Refresh", GUILayout.Width(80))) RefreshList(); + GUILayout.FlexibleSpace(); + EditorGUILayout.EndHorizontal(); + + EditorGUILayout.Space(); + search = EditorGUILayout.TextField(search); + + EditorGUILayout.Space(); + scroll = EditorGUILayout.BeginScrollView(scroll); + foreach (var it in items) { + if (!string.IsNullOrEmpty(search) && it.name.IndexOf(search, StringComparison.OrdinalIgnoreCase) < 0) + continue; + + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField(EditorGUIUtility.ObjectContent(it, typeof(ClusterPrefab)), GUILayout.Height(20)); + if (GUILayout.Button("Select", GUILayout.Width(70))) { + onPicked?.Invoke(it); + Close(); + return; + } + EditorGUILayout.EndHorizontal(); + } + EditorGUILayout.EndScrollView(); + + EditorGUILayout.Space(); + EditorGUILayout.BeginHorizontal(); + if (GUILayout.Button("Cancel")) { onPicked?.Invoke(null); Close(); } + GUILayout.FlexibleSpace(); + EditorGUILayout.EndHorizontal(); + } +} diff --git a/VisualEditor/Editor/BrainPickerWindow.cs.meta b/VisualEditor/Editor/BrainPickerWindow.cs.meta new file mode 100644 index 0000000..b2de114 --- /dev/null +++ b/VisualEditor/Editor/BrainPickerWindow.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 9197e2d322d23b5798ab4aef729815b0 \ No newline at end of file diff --git a/VisualEditor/Editor/ClusterInspector.cs b/VisualEditor/Editor/ClusterInspector.cs new file mode 100644 index 0000000..94273dd --- /dev/null +++ b/VisualEditor/Editor/ClusterInspector.cs @@ -0,0 +1,728 @@ +using System.Collections.Generic; +using System.Linq; +using UnityEditor; + +using UnityEngine; +using UnityEngine.UIElements; +using Unity.Mathematics; +using static Unity.Mathematics.math; + +[CustomEditor(typeof(ClusterPrefab))] +public class ClusterInspector : Editor { + protected static VisualElement mainContainer; + protected static VisualElement inspectorContainer; + + protected bool breakOnWake = false; + + #region Start + + public override VisualElement CreateInspectorGUI() { + ClusterPrefab cluster = target as ClusterPrefab; + + serializedObject.Update(); + + VisualElement root = new(); + //root.style.flexDirection = FlexDirection.Row; // side-by-side layout + //root.style.flexGrow = 1; + //root.style.minHeight = 600; + root.style.paddingLeft = 0; + root.style.paddingRight = 0; + root.style.paddingTop = 0; + root.style.paddingBottom = 0; + + root.styleSheets.Add(Resources.Load("GraphStyles")); + + mainContainer = new() { + // name = "main", + style = { + // flexDirection = FlexDirection.Row, + // flexGrow = 1, + height = 450, + } + }; + GraphView graph = new(); + graph.style.flexGrow = 1; + + inspectorContainer = new VisualElement { + // name = "inspector" + }; + + mainContainer.Add(graph); + mainContainer.Add(inspectorContainer); + root.Add(mainContainer); + + // Run once for initial state (use resolved style width if available) + float initialWidth = root.layout.width > 0 ? root.layout.width : root.contentRect.width; + UpdateLayout(initialWidth); + + // React to size changes of root (or parent if appropriate) + root.RegisterCallback(evt => { + UpdateLayout(evt.newRect.width); + }); + + if (cluster != null) { + cluster.EnsureInitialization(); + graph.SetGraph(null, cluster, cluster.output, inspectorContainer); + } + else + Debug.LogWarning(" No brain!"); + + serializedObject.ApplyModifiedProperties(); + return root; + } + + public class GraphView : VisualElement { + ClusterPrefab cluster; + SerializedObject serializedBrain; + INucleus currentNucleus; + GameObject gameObject; + private List layers = new(); + private readonly Dictionary neuroidPositions = new(); + private bool expandArray = false; + + //Vector2 pan = Vector2.zero; + //float zoom = 1f; + //bool draggingCanvas = false; + //Vector2 lastMouse; + ClusterWrapper currentWrapper; + + public GraphView() { + name = "content"; + style.flexGrow = 1; + + IMGUIContainer imguiContainer = new(OnIMGUI); + imguiContainer.style.position = Position.Absolute; + imguiContainer.style.left = 0; imguiContainer.style.top = 0; + imguiContainer.style.right = 0; imguiContainer.style.bottom = 0; + imguiContainer.pickingMode = PickingMode.Position; + imguiContainer.focusable = true; + Add(imguiContainer); + + //RegisterCallback(OnWheel); + // RegisterCallback(OnMouseDown); + // RegisterCallback(OnMouseMove); + // RegisterCallback(OnMouseUp); + + // Subscribe when added to panel (editor UI ready) + RegisterCallback(evt => Subscribe()); + RegisterCallback(evt => Unsubscribe()); + } + + + bool subscribed = false; + void Subscribe() { + if (subscribed) return; + SceneView.duringSceneGui += OnSceneGUI; + subscribed = true; + SceneView.RepaintAll(); + } + + void Unsubscribe() { + if (!subscribed) return; + SceneView.duringSceneGui -= OnSceneGUI; + subscribed = false; + } + + public void SetGraph(GameObject gameObject, ClusterPrefab brain, INucleus nucleus, VisualElement inspectorContainer) { + this.gameObject = gameObject; + this.cluster = brain; + if (Application.isPlaying == false) + this.serializedBrain = new SerializedObject(brain); + this.currentNucleus = nucleus; + Rebuild(inspectorContainer); + } + + void Rebuild(VisualElement inspectorContainer) { + BuildLayers(); + + if (this.currentNucleus == null) { + inspectorContainer.Clear(); + return; + } + + if (currentWrapper != null) + DestroyImmediate(currentWrapper); + currentWrapper = CreateInstance().Init(this.currentNucleus, cluster); + DrawInspector(inspectorContainer); + } + + private void BuildLayers() { + // A temporary list to track what's been added to layers + this.layers = new(); + int layerIx = 0; + + INucleus selectedNucleus = this.currentNucleus; + if (selectedNucleus == null) + return; + NeuroidLayer currentLayer = new() { ix = layerIx }; + + if (selectedNucleus.receivers != null) { + foreach (INucleus receiver in selectedNucleus.receivers) { + INucleus outputNeuroid = receiver; + if (outputNeuroid != null) { + AddToLayer(currentLayer, outputNeuroid); + // Debug.Log($"layer {layerIx} nucleus {outputNeuroid.name}"); + } + } + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + layerIx++; + currentLayer = new() { ix = layerIx }; + } + + AddToLayer(currentLayer, selectedNucleus); + this.layers.Add(currentLayer); + // Debug.Log($"layer {layerIx} nucleus {selectedNucleus.name}"); + + layerIx++; + currentLayer = new() { ix = layerIx }; + + if (selectedNucleus.synapses != null) { + foreach (Synapse synapse in selectedNucleus.synapses) { + IReceptor input = synapse.nucleus; + AddToLayer(currentLayer, input); + // Debug.Log($"layer {layerIx} nucleus {input.name}"); + } + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + } + } + + private void AddToLayer(NeuroidLayer layer, IReceptor nucleus) { + if (nucleus == null) + return; + layer.neuroids.Add(nucleus); + //nucleus.layerIx = layer.ix; + // Store its position + Vector2Int neuroidPosition = new(layer.ix, layer.neuroids.Count - 1); + neuroidPositions[nucleus] = neuroidPosition; + + } + + + public void OnIMGUI() { + if (currentNucleus == null) + return; + + if (Application.isPlaying == false) + serializedBrain.Update(); + + Handles.BeginGUI(); + DrawGraph(); + Handles.EndGUI(); + + } + + private void DrawGraph() { + float size = 20; + Vector3 position = new(150, 210, 0); + + DrawReceivers(this.currentNucleus, position, size); + DrawSynapses(this.currentNucleus, position, size); + + // Draw selected Nucleus + if (expandArray) { + float maxValue = 0; + foreach (INucleus nucleus in this.currentNucleus.array.nuclei) { + float value = length(nucleus.outputValue); + if (value > maxValue) + maxValue = value; + } + + float spacing = 400f / this.currentNucleus.array.nuclei.Count(); + float margin = 10 + spacing / 2; + float xMin = 150 - size; + float xMax = 150 + size; + float yMin = 10 + margin - size / 2; + float yMax = 400 - margin + size; + Vector3[] verts = new Vector3[4] { + new(xMin, yMin, 0), + new(xMax, yMin, 0), + new(xMax, yMax, 0), + new(xMin, yMax, 0) + }; + Handles.color = Color.black; + Handles.DrawAAConvexPolygon(verts); + int row = 0; + foreach (INucleus nucleus in this.currentNucleus.array.nuclei) { + Vector3 pos = new(150, margin + row * spacing, 0.0f); + Handles.color = Color.white; + //Handles.DrawLine(parentPos, pos); + + Handles.color = Color.white; + Handles.DrawSolidDisc(pos, Vector3.forward, size + 2); + DrawNucleus(nucleus, pos, maxValue, size); + row++; + } + GUIStyle style = new(EditorStyles.label) { + alignment = TextAnchor.UpperCenter, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold, + }; + Vector3 labelPos = new Vector3(150, yMax, 0) - Vector3.down * (size + 10); // below disc along up axis + Handles.Label(labelPos, this.currentNucleus.name, style); + } + else { + Handles.color = Color.white; + Handles.DrawSolidDisc(position, Vector3.forward, size + 2); + DrawNucleus(this.currentNucleus, position, length(this.currentNucleus.outputValue), 20); + } + } + + private void DrawReceivers(INucleus nucleus, Vector3 parentPos, float size) { + int nodeCount = nucleus.receivers.Count(); + + // Determine the maximum value in this layer + // This is used to 'scale' the output value colors of the nuclei + float maxValue = 0; + foreach (INucleus receiver in nucleus.receivers) { + if (receiver is Neuron neuroid) { + float value = length(neuroid.outputValue); + if (value > maxValue) + maxValue = value; + } + } + + // Determine the spacing of the nuclei in the layer + float spacing = 400f / nodeCount; + float margin = 10 + spacing / 2; + + int row = 0; + foreach (INucleus receiver in nucleus.receivers) { + INucleus receiverNucleus = receiver; + if (receiverNucleus == null) + continue; + + Vector3 pos = new(50, margin + row * spacing, 0.0f); + Handles.color = Color.white; + Handles.DrawLine(parentPos, pos); + + DrawNucleus(receiverNucleus, pos, maxValue, size); + row++; + } + } + + private void DrawSynapses(INucleus nucleus, Vector3 parentPos, float size) { + int nodeCount = nucleus.synapses.Count; + + // Determine the maximum value in this layer + // This is used to 'scale' the output value colors of the nuclei + float maxValue = 0; + int neuronCount = 0; + List drawnArrays = new(); + foreach (Synapse synapse in nucleus.synapses) { + if (synapse.nucleus is Neuron neuroid) { + if (drawnArrays.Contains(neuroid.array)) + continue; + drawnArrays.Add(neuroid.array); + + } + float value = length(synapse.nucleus.outputValue) * synapse.weight; + // Debug.Log($"{synapse.nucleus.name}: {value} {length(synapse.nucleus.outputValue)} {synapse.weight}"); + if (value > maxValue) + maxValue = value; + neuronCount++; + } + + // Determine the spacing of the nuclei in the layer + float spacing = 400f / neuronCount; + float margin = 10 + spacing / 2; + + int row = 0; + drawnArrays = new(); + foreach (Synapse synapse in nucleus.synapses) { + if (synapse.nucleus is Neuron neuron) { + if (drawnArrays.Contains(neuron.array)) + continue; + drawnArrays.Add(neuron.array); + } + Vector3 pos = new(250, margin + row * spacing, 0.0f); + Handles.color = Color.white; + Handles.DrawLine(parentPos, pos); + if (synapse.nucleus != null) { + Color color = Color.black; + if (synapse.nucleus.isSleeping) + color = Color.darkRed; + else if (Application.isPlaying) { + float brightness = length(synapse.nucleus.outputValue) * synapse.weight / maxValue; + color = new Color(brightness, brightness, brightness, 1f); + } + DrawNucleus(synapse.nucleus, pos, maxValue, size, color); + } + row++; + } + } + + private void DrawNucleus(IReceptor nucleus, Vector3 position, float maxValue, float size) { + Color color; + if (nucleus.isSleeping) + color = Color.darkRed; + else { + if (Application.isPlaying) { + float brightness = length(nucleus.outputValue) / maxValue; + color = new Color(brightness, brightness, brightness, 1f); + } + else + color = Color.black; + } + DrawNucleus(nucleus, position, maxValue, size, color); + } + + private void DrawNucleus(IReceptor nucleus, Vector3 position, float maxValue, float size, Color color) { + if (nucleus is MemoryCell memory) { + Handles.color = Color.white; + Handles.DrawWireDisc(position + Vector3.right * 10, Vector3.forward, size); + } + + Handles.color = color; + Handles.DrawSolidDisc(position, Vector3.forward, size); + + Handles.color = Color.white; + // Position the label in front of the disc + Vector3 labelPosition = position + (Vector3.forward * 0.1f); + + GUIStyle style = new(EditorStyles.label) { + alignment = TextAnchor.MiddleCenter, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold, + }; + if (nucleus is INucleus neuron) { + if (neuron.array == null || neuron.array.nuclei == null || neuron.array.nuclei.Count() == 0) + neuron.array = new NucleusArray(neuron); + + if ((!expandArray || neuron.array.nuclei.First() != this.currentNucleus) && neuron.array.nuclei.Count() > 1) { + Handles.Label(labelPosition, neuron.array.nuclei.Count().ToString(), style); + } + if (expandArray && neuron.array.nuclei.First() == this.currentNucleus) { + int arrayIx = 0; + foreach (INucleus n in neuron.array.nuclei) { + if (n == neuron) + break; + arrayIx++; + } + Handles.Label(labelPosition, $"[{arrayIx}]", style); + } + else { + style.alignment = TextAnchor.UpperCenter; + Vector3 labelPos = position - Vector3.down * (size + 10f); // below disc along up axis + Handles.Label(labelPos, nucleus.name, style); + } + + if (nucleus is Cluster cluster) { + Handles.color = Color.white; + Handles.DrawWireDisc(position, Vector3.forward, size + 10); + } + } + else { + style.alignment = TextAnchor.UpperCenter; + Vector3 labelPos = position - Vector3.down * (size + 10); // below disc along up axis + Handles.Label(labelPos, nucleus.name, style); + } + + Rect neuronRect = new(position.x - size, position.y - size, size * 2, size * 2); + int id = GUIUtility.GetControlID(FocusType.Passive); + Event e = Event.current; + EventType et = e.GetTypeForControl(id); + if (e != null && neuronRect.Contains(e.mousePosition)) { + // Process Hover + HandleMouseHover(nucleus, neuronRect); + // Process click + if (e.type == EventType.MouseDown && e.button == 0) { + // Consume the event so the scene doesn't also handle it + e.Use(); + HandleClicked(nucleus); + } + } + } + + private void HandleMouseHover(IReceptor nucleus, Rect rect) { + GUIContent tooltip; + if (nucleus is INucleus n) { + tooltip = new( + $"{nucleus.name}" + + $"\nsynapse count {n.synapses.Count}" + + $"\nValue: {nucleus.outputValue}"); + } + else { + tooltip = new( + $"{nucleus.name}" + + $"\nValue: {nucleus.outputValue}"); + } + + Vector2 mousePosition = Event.current.mousePosition; + + // Display tooltip with some offset + Vector2 tooltipSize = GUI.skin.box.CalcSize(tooltip); + Rect tooltipRect = new Rect(mousePosition.x + 10, mousePosition.y + 10, tooltipSize.x, tooltipSize.y); + + GUI.Box(tooltipRect, tooltip); + } + + private void HandleClicked(IReceptor nucleus) { + if (nucleus == this.currentNucleus) { + if (nucleus is INucleus n) { + expandArray = !expandArray; + return; + } + } + else if (nucleus is INucleus n) { + this.currentNucleus = n; + BuildLayers(); + } + } + + void DrawInspector(VisualElement inspectorContainer) { + if (inspectorContainer == null) + return; + + inspectorContainer.Clear(); + if (this.currentNucleus == null) + return; + + // create a SerializedObject wrapper so Unity inspector controls work (and Undo) + SerializedObject so = new(currentWrapper); + IMGUIContainer container = new(() => { + if (so.targetObject == null) + return; + so.Update(); + + if (this.currentNucleus == null) + return; + + this.currentNucleus.name = EditorGUILayout.TextField(this.currentNucleus.name); + if (this.currentNucleus is Neuron neuroid) { + if (this.currentNucleus is MemoryCell memory) { + } + else { + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField("Activation Curve", GUILayout.Width(150)); + if (neuroid.curveMax > 0) + EditorGUILayout.CurveField(neuroid.curve, Color.cyan, new Rect(0, 0, 1, neuroid.curveMax)); + else + EditorGUILayout.CurveField(neuroid.curve, Color.cyan, new Rect(0, neuroid.curveMax, 1, -neuroid.curveMax)); + neuroid.curvePreset = (Neuron.CurvePresets)EditorGUILayout.EnumPopup(neuroid.curvePreset, GUILayout.Width(100)); + EditorGUILayout.EndHorizontal(); + } + + if (neuroid.array == null || neuroid.array.nuclei == null || neuroid.array.nuclei.Count() == 0) + neuroid.array = new NucleusArray(neuroid); + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.IntField("Array size", neuroid.array.nuclei.Count()); + if (GUILayout.Button("Add")) + neuroid.array.AddNucleus(); + if (GUILayout.Button("Del")) + neuroid.array.RemoveNucleus(); + EditorGUILayout.EndHorizontal(); + } + + if (Application.isPlaying) + EditorGUILayout.FloatField("Output", length(this.currentNucleus.outputValue)); + else + EditorGUILayout.LabelField(" "); + + if (this.currentNucleus.synapses.Count > 0) { + EditorGUILayout.LabelField("Synapses"); + Synapse[] synapses = this.currentNucleus.synapses.ToArray(); + foreach (Synapse synapse in synapses) { + if (synapse.nucleus != null) { + EditorGUILayout.Space(); + + //EditorGUI.BeginDisabledGroup(synapse.nucleus.isSleeping); + if (Application.isPlaying) + EditorGUILayout.FloatField(synapse.nucleus.name, length(synapse.nucleus.outputValue) * synapse.weight); + else { + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField(synapse.nucleus.name); + if (GUILayout.Button("Disconnect")) + synapse.nucleus.RemoveReceiver(this.currentNucleus); + EditorGUILayout.EndHorizontal(); + } + + EditorGUI.indentLevel++; + synapse.weight = EditorGUILayout.FloatField("Weight", synapse.weight); + EditorGUI.indentLevel--; + //EditorGUI.EndDisabledGroup(); + } + } + } + + EditorGUILayout.Space(); + + ConnectNucleus(this.cluster, this.currentNucleus); + if (GUILayout.Button("Add Input Neuron")) + AddInputNeuron(this.currentNucleus); + if (GUILayout.Button("Add Input MemoryCell")) + AddInputMemoryCell(this.currentNucleus); + if (GUILayout.Button("Add Input Cluster")) + AddCluster(this.currentNucleus); + + EditorGUILayout.Space(); + + if (GUILayout.Button("Delete this neuron")) + DeleteNeuron(this.currentNucleus); + + if (this.currentNucleus is Cluster subCluster) { + if (GUILayout.Button("Edit Cluster")) + EditCluster(subCluster); + } + + // if (this.gameObject != null) { + // Vector3 worldVector = this.gameObject.transform.TransformVector(this.currentNucleus.outputValue); + // //Debug.DrawRay(this.gameObject.transform.position, worldVector, Color.yellow); + // Handles.color = Color.yellow; + // Handles.DrawLine(this.gameObject.transform.position, this.gameObject.transform.position + worldVector); + // } + }); + + inspectorContainer.Add(container); + } + + void OnSceneGUI(SceneView sceneView) { + if (this.gameObject != null) { + Vector3 worldVector = this.gameObject.transform.TransformVector(this.currentNucleus.outputValue); + Handles.color = Color.yellow; + Handles.DrawLine(this.gameObject.transform.position, this.gameObject.transform.position + worldVector); + } + } + + protected virtual void AddInputNeuron(INucleus nucleus) { + Neuron newNeuroid = new(this.cluster, "New neuron"); + newNeuroid.AddReceiver(nucleus); + this.currentNucleus = newNeuroid; + BuildLayers(); + } + + protected virtual void DeleteNeuron(INucleus nucleus) { + if (nucleus == null) + return; + if (nucleus.cluster != null) + this.currentNucleus = nucleus.cluster.output; + foreach (INucleus receiver in nucleus.receivers) { + if (receiver != null) { + this.currentNucleus = receiver; + break; + } + } + Neuron.Delete(nucleus); + BuildLayers(); + } + + protected virtual void AddInputMemoryCell(INucleus nucleus) { + MemoryCell newMemory = new(this.cluster, "New memory cell"); + newMemory.AddReceiver(nucleus); + this.currentNucleus = newMemory; + BuildLayers(); + } + + protected virtual void AddCluster(INucleus nucleus) { + BrainPickerWindow.ShowPicker(brain => OnClusterPicked(nucleus, brain), "Select Cluster"); + } + + private void OnClusterPicked(INucleus nucleus, ClusterPrefab subCluster) { + Cluster subclusterInstance = new(this.cluster, subCluster); + //this.cluster.AddSubCluster(subclusterInstance); + //this.cluster.nuclei.Add(subclusterInstance); + subclusterInstance.AddReceiver(nucleus); + } + + private void EditCluster(Cluster subCluster) { + //var currentActiveObject = Selection.activeObject; + Selection.activeObject = subCluster.prefab; + EditorGUIUtility.PingObject(subCluster.prefab); + var editor = Editor.CreateEditor(subCluster.prefab); + //Selection.activeObject = currentActiveObject; + } + + // Connect to another nucleus in the same cluster + protected virtual void ConnectNucleus(ClusterPrefab cluster, INucleus nucleus) { + if (cluster == null) + return; + + IEnumerable synapseNuclei = this.currentNucleus.synapses.Select(synapse => synapse.nucleus != null ? synapse.nucleus.name : ""); + //IEnumerable perceptei = this.currentNucleus.brain.perceptei.Select(i => i.name).Except(synapseNuclei); + IEnumerable nuclei = cluster.nuclei.Select(i => i.name).Except(synapseNuclei); + //string[] names = perceptei.Concat(nuclei).ToArray(); + string[] names = nuclei.ToArray(); + int selectedIndex = -1; + selectedIndex = EditorGUILayout.Popup("Connect to", selectedIndex, names); + if (selectedIndex >= 0) { + // if (selectedIndex < perceptei.Count()) { + // Nucleus n = this.currentNucleus.brain.perceptei[selectedIndex]; + // n.AddReceiver(this.currentNucleus); + // } + // else { + // Nucleus n = this.currentNucleus.brain.nuclei[selectedIndex - perceptei.Count()]; + // n.AddReceiver(this.currentNucleus); + // } + IReceptor receptor = cluster.nuclei[selectedIndex]; + receptor.AddReceiver(this.currentNucleus); + } + } + + protected virtual void DisconnectNucleus(Neuron nucleus) { + if (this.currentNucleus.cluster == null) + return; + string[] names = this.currentNucleus.synapses.Select(synapse => synapse.nucleus.name).ToArray(); + int selectedIndex = -1; + selectedIndex = EditorGUILayout.Popup("Disconnect from", selectedIndex, names); + //if (selectedIndex >= 0 && selectedIndex < this.currentNucleus.brain.perceptei.Count) { + if (selectedIndex >= 0 && selectedIndex < this.currentNucleus.cluster.nuclei.Count) { + Synapse synapse = this.currentNucleus.synapses[selectedIndex]; + synapse.nucleus.RemoveReceiver(this.currentNucleus); + } + } + } + + #endregion Start + + #region Update + + private void UpdateLayout(float containerWidth) { + if (containerWidth > 600f) { + mainContainer.style.flexDirection = FlexDirection.Row; + inspectorContainer.style.width = 300; // fixed sidebar width + inspectorContainer.style.flexGrow = 0; + } + else { + mainContainer.style.flexDirection = FlexDirection.Column; + inspectorContainer.style.width = Length.Percent(100); // full width below + inspectorContainer.style.flexDirection = FlexDirection.Column; + inspectorContainer.style.flexGrow = 1; // can set 0 or keep as needed + } + } + + #endregion Update +} + +public class NeuroidLayer { + public int ix = 0; + public List neuroids = new(); +} + +public class ClusterWrapper : ScriptableObject { + // expose fields that map to GraphNode + //public string title; + public Vector2 position; + INucleus node; + ClusterPrefab graph; // needed to write back and mark dirty + + public ClusterWrapper Init(INucleus node, ClusterPrefab graphAsset) { + this.node = node; + this.graph = graphAsset; + //this.title = " A " + node.name; + //position = node.position; + return this; + } + void OnValidate() { + if (node != null) { + //node.name = title; + //node.position = position; +#if UNITY_EDITOR + if (graph != null) + UnityEditor.EditorUtility.SetDirty(graph); +#endif + } + } +} \ No newline at end of file diff --git a/VisualEditor/Editor/ClusterInspector.cs.meta b/VisualEditor/Editor/ClusterInspector.cs.meta new file mode 100644 index 0000000..a1a18f5 --- /dev/null +++ b/VisualEditor/Editor/ClusterInspector.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 1fc1fb7db9f7ad54a87d31313e7f457d \ No newline at end of file diff --git a/VisualEditor/Editor/NanoBrainComponent_Editor.cs b/VisualEditor/Editor/NanoBrainComponent_Editor.cs new file mode 100644 index 0000000..253993a --- /dev/null +++ b/VisualEditor/Editor/NanoBrainComponent_Editor.cs @@ -0,0 +1,129 @@ +using UnityEditor; +using UnityEditor.UIElements; +using UnityEngine; +using UnityEngine.UIElements; + +[CustomEditor(typeof(NanoBrainComponent))] +public class NanoBrainComponent_Editor : Editor { + protected static VisualElement mainContainer; + protected static VisualElement inspectorContainer; + + protected NanoBrainComponent component; + private SerializedProperty brainProp; + + ClusterInspector.GraphView board; + + public void OnEnable() { + component = target as NanoBrainComponent; + + if (Application.isPlaying == false) + brainProp = serializedObject.FindProperty(nameof(NanoBrainComponent.defaultBrain)); + + } + + public override VisualElement CreateInspectorGUI() { + //NanoBrainComponent component = target as NanoBrainComponent; + ClusterPrefab brain = Application.isPlaying ? component.brain : component.defaultBrain; + + if (Application.isPlaying == false) + serializedObject.Update(); + + + VisualElement root = new(); + root.style.flexDirection = FlexDirection.Column; // side-by-side layout + root.style.flexGrow = 1; + root.style.minHeight = 600; + root.style.paddingLeft = 0; + root.style.paddingRight = 0; + root.style.paddingTop = 0; + root.style.paddingBottom = 0; + + root.styleSheets.Add(Resources.Load("GraphStyles")); + + if (Application.isPlaying == false) { + PropertyField brainField = new(brainProp) { + label = "Nano Brain" + }; + root.Add(brainField); + } + + mainContainer = new() { + name = "main", + style = { + flexDirection = FlexDirection.Row, + flexGrow = 1, + minHeight = 500, + } + }; + board = new ClusterInspector.GraphView(); + board.style.flexGrow = 1; + mainContainer.Add(board); + + inspectorContainer = new VisualElement { + name = "inspector", + style = { + width = 400, + } + }; + + mainContainer.Add(inspectorContainer); + root.Add(mainContainer); + + // Run once for initial state (use resolved style width if available) + float initialWidth = root.layout.width > 0 ? root.layout.width : root.contentRect.width; + UpdateLayout(initialWidth); + + // React to size changes of root (or parent if appropriate) + root.RegisterCallback(evt => { + UpdateLayout(evt.newRect.width); + }); + + if (brain != null && board != null) + board.SetGraph(component.gameObject, brain, brain.output, inspectorContainer); + // else + // Debug.LogWarning(" No brain!"); + + if (Application.isPlaying == false) + serializedObject.ApplyModifiedProperties(); + return root; + } + + // void OnSceneGUI() { + // if (Application.isPlaying && board != null) + // board.OnIMGUI(); + // } + + void OnSceneGui(SceneView sv) { + if (Application.isPlaying == false) + return; + // May need some throttling here... + if (board != null) { + Debug.Log("."); + board.OnIMGUI(); + } + + // EditorApplication.delayCall = UpdateInspectorUI; + } + + void UpdateInspectorUI() { + if (board != null) { + Debug.Log("."); + board.OnIMGUI(); + } + } + + private void UpdateLayout(float containerWidth) { + if (containerWidth > 800f) { + mainContainer.style.flexDirection = FlexDirection.Row; + inspectorContainer.style.width = 400; // fixed sidebar width + inspectorContainer.style.flexGrow = 0; + } + else { + mainContainer.style.flexDirection = FlexDirection.Column; + inspectorContainer.style.width = Length.Percent(100); // full width below + inspectorContainer.style.flexDirection = FlexDirection.Column; + inspectorContainer.style.flexGrow = 1; // can set 0 or keep as needed + } + } + +} \ No newline at end of file diff --git a/VisualEditor/Editor/NanoBrainComponent_Editor.cs.meta b/VisualEditor/Editor/NanoBrainComponent_Editor.cs.meta new file mode 100644 index 0000000..eaf830b --- /dev/null +++ b/VisualEditor/Editor/NanoBrainComponent_Editor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: f05072314d39990639a2dbf99f322664 \ No newline at end of file diff --git a/VisualEditor/Editor/NanoBrainEditor.cs b/VisualEditor/Editor/NanoBrainEditor.cs new file mode 100644 index 0000000..230bfa1 --- /dev/null +++ b/VisualEditor/Editor/NanoBrainEditor.cs @@ -0,0 +1,511 @@ +/* +using UnityEditor; +using UnityEngine; +using UnityEngine.UIElements; +using UnityEditor.Callbacks; +using System.Linq; +using System.Collections.Generic; + +public class NucleusLayer { + public int ix = 0; + public List neuroids = new(); +} + +public class NanoBrainEditor : EditorWindow { + public NanoBrain brain; + + public static VisualElement inspectorContainer; + + [MenuItem("Window/NanoBrain Editor")] + public static void ShowWindow() { + GetWindow("NanoBrain Editor"); + } + + public static void Open(NanoBrain asset) { + NanoBrainEditor editor = GetWindow("NanoBrain Editor"); + editor.brain = asset; + editor.Show(); + } + + GraphBoardView board; + + private void OnEnable() { + OnFocus(); + } + private void OnFocus() { + if (brain == null) { + // brain = CreateInstance(); + // EditorUtility.SetDirty(brain); + return; + } + + VisualElement root = rootVisualElement; + root.Clear(); + root.styleSheets.Add(Resources.Load("GraphStyles")); + + VisualElement main = new() { + name = "main", + style = { + flexDirection = FlexDirection.Row, + flexGrow = 1 + } + }; + board = new GraphBoardView(); + board.style.flexGrow = 1; + inspectorContainer = new VisualElement { + name = "inspector", + style = { + width = 400 + } + }; + + main.Add(board); + main.Add(inspectorContainer); + root.Add(main); + + board.SetGraph(brain, brain.root); + + } + +} + +public class GraphBoardView : VisualElement { + NanoBrain brain; + SerializedObject serializedBrain; + Nucleus currentNucleus; + private List layers = new(); + private Dictionary neuroidPositions = new(); + + Vector2 pan = Vector2.zero; + //float zoom = 1f; + bool draggingCanvas = false; + Vector2 lastMouse; + GraphNodeWrapper currentWrapper; + + public GraphBoardView() { + name = "content"; + style.flexGrow = 1; + + IMGUIContainer imguiContainer = new(OnIMGUI); + imguiContainer.style.position = Position.Absolute; + imguiContainer.style.left = 0; imguiContainer.style.top = 0; + imguiContainer.style.right = 0; imguiContainer.style.bottom = 0; + imguiContainer.pickingMode = PickingMode.Position; + imguiContainer.focusable = true; + Add(imguiContainer); + + //RegisterCallback(OnWheel); + RegisterCallback(OnMouseDown); + RegisterCallback(OnMouseMove); + RegisterCallback(OnMouseUp); + } + + public void SetGraph(NanoBrain brain, Nucleus nucleus) { + this.brain = brain; + this.serializedBrain = new SerializedObject(brain); + this.currentNucleus = nucleus; + Rebuild(); + } + + void Rebuild() { + BuildLayers(); + + if (currentNucleus == null) { + NanoBrainEditor.inspectorContainer.Clear(); + return; + } + + if (currentWrapper != null) + Object.DestroyImmediate(currentWrapper); + currentWrapper = ScriptableObject.CreateInstance().Init(currentNucleus, brain); + DrawInspector(); + } + + private void BuildLayers() { + // A temporary list to track what's been added to layers + this.layers = new(); + int layerIx = 0; + + Nucleus selectedNucleus = this.currentNucleus; + if (selectedNucleus == null) + return; + NeuroidLayer currentLayer = new() { ix = layerIx }; + + //foreach (Nucleus outputNeuroid in selectedNucleus.receivers) { + foreach (Receiver receiver in selectedNucleus.receivers) { + Nucleus outputNeuroid = receiver.nucleus; + if (outputNeuroid != null) { + AddToLayer(currentLayer, outputNeuroid); + // Debug.Log($"layer {layerIx} nucleus {outputNeuroid.name}"); + } + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + layerIx++; + currentLayer = new() { ix = layerIx }; + } + + AddToLayer(currentLayer, selectedNucleus); + this.layers.Add(currentLayer); + // Debug.Log($"layer {layerIx} nucleus {selectedNucleus.name}"); + + layerIx++; + currentLayer = new() { ix = layerIx }; + + //foreach (Nucleus input in selectedNucleus.synapses.Keys) { + foreach (Synapse synapse in selectedNucleus.synapses) { + Nucleus input = synapse.nucleus; + AddToLayer(currentLayer, input); + // Debug.Log($"layer {layerIx} nucleus {input.name}"); + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + } + } + + private void AddToLayer(NeuroidLayer layer, Nucleus nucleus) { + if (nucleus == null) + return; + layer.neuroids.Add(nucleus); + nucleus.layerIx = layer.ix; + // Store its position + Vector2Int neuroidPosition = new(layer.ix, layer.neuroids.Count - 1); + neuroidPositions[nucleus] = neuroidPosition; + + } + + // basic pan/zoom handling + // void OnWheel(WheelEvent e) { + // if (e.ctrlKey) { + // float delta = -e.delta.y * 0.001f; + // zoom = Mathf.Clamp(zoom + delta, 0.25f, 2f); + // content.transform.rotation = Quaternion.identity; // keep transform accessible + // content.transform.scale = new Vector3(zoom, zoom, 1); + // e.StopPropagation(); + // } + // else { + // pan += e.delta; + // content.style.left = pan.x; + // content.style.top = pan.y; + // } + // } + + void OnMouseDown(MouseDownEvent e) { + if (e.button == 2) { draggingCanvas = true; lastMouse = e.mousePosition; e.StopPropagation(); } + } + void OnMouseMove(MouseMoveEvent e) { + if (draggingCanvas) { + var delta = e.mousePosition - lastMouse; + pan += delta; + //content.style.left = pan.x; + //content.style.top = pan.y; + lastMouse = e.mousePosition; + } + } + void OnMouseUp(MouseUpEvent e) { if (e.button == 2) draggingCanvas = false; } + + void OnIMGUI() { + if (currentNucleus == null) + return; + + serializedBrain.Update(); + + Handles.BeginGUI(); + foreach (NeuroidLayer layer in layers) + DrawLayer(layer); + Handles.EndGUI(); + + } + + private void DrawLayer(NeuroidLayer layer) { + int nodeCount = layer.neuroids.Count; + float maxValue = 0; + foreach (Nucleus nucleus in layer.neuroids) { + if (nucleus is Neuroid neuroid) { + float value = neuroid.outputValue.magnitude; + if (value > maxValue) + maxValue = value; + } + } + float spacing = 400f / nodeCount; + float margin = 10 + spacing / 2; + foreach (Nucleus layerNucleus in layer.neuroids) { + Vector2Int layerNeuroidPos = this.neuroidPositions[layerNucleus]; + Vector3 parentPos = new(100 + layerNeuroidPos.x * 100, margin + layerNeuroidPos.y * spacing, 0.1f); + + //int i = 0; + float inputSpacing = 400f / layerNucleus.synapses.Count; + float inputMargin = 10 + inputSpacing / 2; + // int minStale = 10000; + //foreach ((Nucleus nucleus, float weight) in layerNucleus.synapses) { + foreach (Synapse synapse in layerNucleus.synapses) { + Nucleus nucleus = synapse.nucleus; + if (nucleus != null) { + float weight = synapse.weight; + if (this.neuroidPositions.ContainsKey(nucleus)) { + Vector2Int inputNeuroidPos = this.neuroidPositions[nucleus]; + if (inputNeuroidPos.x == layerNeuroidPos.x + 1) { + Vector3 pos = new(100 + inputNeuroidPos.x * 100, inputMargin + inputNeuroidPos.y * inputSpacing, 0.0f); + + float brightness = weight / 10.0f; + Handles.color = new Color(brightness, brightness, brightness); + Handles.DrawLine(parentPos, pos); + } + } + // if (nucleus is Neuroid neuroid && neuroid.stale < minStale) + // minStale = neuroid.stale; + } + } + + // if (layerNucleus.synapses.Count > 0 && minStale > 2 && layerNucleus.stale < 3) + // Debug.LogWarning($"Strange {minStale} is big duing update"); + + + float size = 20; + if (layerNucleus.isSleeping) + Handles.color = Color.darkRed; + else { + float brightness = layerNucleus.outputValue.magnitude / maxValue; + Handles.color = new Color(brightness, brightness, brightness); + } + Handles.DrawSolidDisc(parentPos, Vector3.forward, size); + Vector3 labelPos = parentPos - Vector3.down * (size + 0.2f); // below disc along up axis + GUIStyle style = new GUIStyle(EditorStyles.label) { + alignment = TextAnchor.UpperCenter, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold + }; + Handles.Label(labelPos, layerNucleus.name, style); + + Rect neuronRect = new(parentPos.x - size, parentPos.y - size, size * 2, size * 2); + int id = GUIUtility.GetControlID(FocusType.Passive); + Event e = Event.current; + EventType et = e.GetTypeForControl(id); + if (e != null && neuronRect.Contains(e.mousePosition)) { + // Process Hover + HandleMouseHover(layerNucleus, neuronRect); + // Process click + // Debug.Log($"{et} {e.type}"); + if (e.type == EventType.MouseDown && e.button == 0) { + // Consume the event so the scene doesn't also handle it + e.Use(); + HandleDiscClicked(layerNucleus); + } + } + } + } + + private void HandleMouseHover(Nucleus neuroid, Rect rect) { + GUIContent tooltip; + // if (neuroid is SensoryNeuroid sensoryNeuroid) { + // tooltip = new( + // $"{sensoryNeuroid.name}" + + // $"\nThing {sensoryNeuroid.receptor.thingType}" + + // $"\nValue: {neuroid.outputValue}"); + // } + // else { + tooltip = new( + $"{neuroid.name}" + + $"\nsynapse count {neuroid.synapses.Count}" + + $"\nValue: {neuroid.outputValue}"); + // } + + Vector2 mousePosition = Event.current.mousePosition; + + // Display tooltip with some offset + Vector2 tooltipSize = GUI.skin.box.CalcSize(tooltip); + Rect tooltipRect = new Rect(mousePosition.x + 10, mousePosition.y + 10, tooltipSize.x, tooltipSize.y); + + GUI.Box(tooltipRect, tooltip); + } + + private void HandleDiscClicked(Nucleus nucleus) { + this.currentNucleus = nucleus; + BuildLayers(); + } + + + void DrawInspector() { + if (NanoBrainEditor.inspectorContainer == null) + return; + + NanoBrainEditor.inspectorContainer.Clear(); + if (this.currentNucleus == null) + return; + + // create a SerializedObject wrapper so Unity inspector controls work (and Undo) + SerializedObject so = new SerializedObject(currentWrapper); + IMGUIContainer container = new IMGUIContainer(() => { + so.Update(); + currentNucleus.name = EditorGUILayout.TextField(currentNucleus.name); + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField("Output Value", GUILayout.Width(100)); + EditorGUILayout.Vector3Field(GUIContent.none, currentNucleus.outputValue); + EditorGUILayout.EndHorizontal(); + if (currentNucleus.synapses.Count > 0) { + EditorGUILayout.LabelField("Synapses"); + EditorGUI.indentLevel++; + + //List nuclei = currentNucleus.synapses.Keys.ToList(); + // foreach (Nucleus nucleus in nuclei) { + foreach (Synapse synapse in currentNucleus.synapses) { + EditorGUI.BeginDisabledGroup(synapse.nucleus.isSleeping); + + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField(synapse.nucleus.name, GUILayout.Width(120)); + EditorGUI.indentLevel--; + EditorGUILayout.LabelField("Weight", GUILayout.Width(45)); + // float weight = currentNucleus.synapses[nucleus]; + // currentNucleus.synapses[nucleus] = EditorGUILayout.FloatField(weight, GUILayout.Width(40)); + synapse.weight = EditorGUILayout.FloatField(synapse.weight, GUILayout.Width(40)); + EditorGUI.indentLevel++; + EditorGUILayout.Vector3Field(GUIContent.none, synapse.nucleus.outputValue, GUILayout.Width(180)); + EditorGUILayout.EndHorizontal(); + + EditorGUI.EndDisabledGroup(); + } + EditorGUI.indentLevel--; + } + if (GUILayout.Button("Add Neuron")) + AddInputNeuron(currentNucleus); + + }); + + NanoBrainEditor.inspectorContainer.Add(container); + } + + protected virtual void AddInputNeuron(Nucleus receiver) { + Neuroid newNeuroid = new(brain, "New neuron"); + newNeuroid.AddReceiver(receiver); + Rebuild(); + } + + private Vector3 NodePosition(Nucleus nucleus, int layerNodeCount = 1) { + if (this.neuroidPositions.ContainsKey(nucleus)) { + Vector2Int nucleusPos = this.neuroidPositions[nucleus]; + return NodePosition(nucleusPos, layerNodeCount); + } + else { + return Vector3.zero; + } + } + private Vector3 NodePosition(Vector2Int location, int layerNodeCount = 1) { + float spacing = 400f / layerNodeCount; + float margin = 10 + spacing / 2; + float size = 20; + Vector3 parentPos = new(100 + location.x * 100 - size, margin + location.y * spacing - size, 0.1f); + return parentPos; + } + + + // public void CreateEdge(string fromId, string toId) { + // if (fromId == toId) return; + // Undo.RecordObject(graph, "Create Edge"); + // graph.edges.Add(new GraphEdge { fromNodeId = fromId, toNodeId = toId }); + // EditorUtility.SetDirty(graph); + // Rebuild(); + // } +} + + +public class NodeView : VisualElement { + Nucleus data; + GraphBoardView board; + Label titleLabel; + //bool dragging = false; + Vector2 localDragStart; + + public NodeView(Nucleus node, GraphBoardView boardView) { + data = node; + board = boardView; + name = "node"; + style.width = 20; //node.size.x; + style.height = 20; //node.size.y; + + titleLabel = new Label(node.name) { name = "title" }; + Add(titleLabel); + + // ports + // var outPort = new Button(() => StartEdgeDrag(true)) { text = "◀", name = "out" }; + // var inPort = new Button(() => StartEdgeDrag(false)) { text = "▶", name = "in" }; + // Add(outPort); + // Add(inPort); + + RegisterCallback(OnMouseDown); + // RegisterCallback(OnMouseMove); + RegisterCallback(OnMouseUp); + //RegisterCallback(e => dragging = false); + } + + // void StartEdgeDrag(bool isOutput) { + // // simplified: on first click store source; on second click on target port call board.CreateEdge + // if (EdgeDragState.active == null) EdgeDragState.active = new EdgeDragState { fromNode = data, fromIsOutput = isOutput }; + // else { + // var src = EdgeDragState.active.fromNode; + // if (src != null && src.id != data.id) board.CreateEdge(src.id, data.id); + // EdgeDragState.active = null; + // } + // } + + void OnMouseDown(MouseDownEvent e) { + if (e.button == 0 && e.target == this) { + //dragging = true; + localDragStart = e.mousePosition; + e.StopPropagation(); + } + } + // void OnMouseMove(MouseMoveEvent e) { + // if (!dragging) return; + // var delta = e.mousePosition - localDragStart; + // var worldPos = new Vector2(layout.x + delta.x, layout.y + delta.y); + // style.left = worldPos.x; + // style.top = worldPos.y; + // // commit on every move + // board.UpdateNodePosition(data, worldPos); + // } + void OnMouseUp(MouseUpEvent e) { + //dragging = false; + } +} + + +public class GraphNodeWrapper : ScriptableObject { + // expose fields that map to GraphNode + public string title; + public Vector2 position; + Nucleus node; + NanoBrain graph; // needed to write back and mark dirty + + public GraphNodeWrapper Init(Nucleus node, NanoBrain graphAsset) { + this.node = node; + this.graph = graphAsset; + this.title = " A " + node.name; + //position = node.position; + return this; + } + void OnValidate() { + if (node != null) { + node.name = title; + //node.position = position; +#if UNITY_EDITOR + if (graph != null) + UnityEditor.EditorUtility.SetDirty(graph); +#endif + } + } +} +//static class EdgeDragState { public static EdgeDragState active; public GraphNode fromNode; public bool fromIsOutput; } + +public static class OpenAssetHandler { + // Called when an asset is double-clicked or opened. + [OnOpenAsset] + public static bool OpenMyScriptableObject(int instanceID, int line) { + NanoBrain obj = EditorUtility.EntityIdToObject(instanceID) as NanoBrain; + if (obj != null) { + NanoBrainEditor.Open(obj); + return true; // handled + } + return false; // let Unity open normally + } +} +*/ \ No newline at end of file diff --git a/VisualEditor/Editor/NanoBrainEditor.cs.meta b/VisualEditor/Editor/NanoBrainEditor.cs.meta new file mode 100644 index 0000000..99dedcd --- /dev/null +++ b/VisualEditor/Editor/NanoBrainEditor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: c57f78e25f0e55b96a50fd5592b26317 \ No newline at end of file diff --git a/VisualEditor/Editor/NanoBrainInspector.cs b/VisualEditor/Editor/NanoBrainInspector.cs new file mode 100644 index 0000000..447fa70 --- /dev/null +++ b/VisualEditor/Editor/NanoBrainInspector.cs @@ -0,0 +1,645 @@ +/* +using System.Collections.Generic; +using System.Linq; +using UnityEditor; + +using UnityEngine; +using UnityEngine.UIElements; + +[CustomEditor(typeof(NanoBrain))] +public class NanoBrainInspector : Editor { + protected static VisualElement mainContainer; + protected static VisualElement inspectorContainer; + + protected bool breakOnWake = false; + + #region Start + + public override VisualElement CreateInspectorGUI() { + NanoBrain brain = target as NanoBrain; + + serializedObject.Update(); + + VisualElement root = new(); + //root.style.flexDirection = FlexDirection.Row; // side-by-side layout + //root.style.flexGrow = 1; + //root.style.minHeight = 600; + root.style.paddingLeft = 0; + root.style.paddingRight = 0; + root.style.paddingTop = 0; + root.style.paddingBottom = 0; + + root.styleSheets.Add(Resources.Load("GraphStyles")); + + mainContainer = new() { + // name = "main", + style = { + // flexDirection = FlexDirection.Row, + // flexGrow = 1, + height = 450, + } + }; + GraphView graph = new(); + graph.style.flexGrow = 1; + + inspectorContainer = new VisualElement { + // name = "inspector" + }; + + mainContainer.Add(graph); + mainContainer.Add(inspectorContainer); + root.Add(mainContainer); + + // Run once for initial state (use resolved style width if available) + float initialWidth = root.layout.width > 0 ? root.layout.width : root.contentRect.width; + UpdateLayout(initialWidth); + + // React to size changes of root (or parent if appropriate) + root.RegisterCallback(evt => { + UpdateLayout(evt.newRect.width); + }); + + if (brain != null) + graph.SetGraph(null, brain, brain.output, inspectorContainer); + else + Debug.LogWarning(" No brain!"); + + serializedObject.ApplyModifiedProperties(); + return root; + } + + public class GraphView : VisualElement { + NanoBrain brain; + SerializedObject serializedBrain; + INucleus currentNucleus; + GameObject gameObject; + private List layers = new(); + private readonly Dictionary neuroidPositions = new(); + + Vector2 pan = Vector2.zero; + //float zoom = 1f; + bool draggingCanvas = false; + Vector2 lastMouse; + GraphNodeWrapper currentWrapper; + + public GraphView() { + name = "content"; + style.flexGrow = 1; + + IMGUIContainer imguiContainer = new(OnIMGUI); + imguiContainer.style.position = Position.Absolute; + imguiContainer.style.left = 0; imguiContainer.style.top = 0; + imguiContainer.style.right = 0; imguiContainer.style.bottom = 0; + imguiContainer.pickingMode = PickingMode.Position; + imguiContainer.focusable = true; + Add(imguiContainer); + + //RegisterCallback(OnWheel); + RegisterCallback(OnMouseDown); + RegisterCallback(OnMouseMove); + RegisterCallback(OnMouseUp); + } + + public void SetGraph(GameObject gameObject, NanoBrain brain, Nucleus nucleus, VisualElement inspectorContainer) { + this.gameObject = gameObject; + this.brain = brain; + if (Application.isPlaying == false) + this.serializedBrain = new SerializedObject(brain); + this.currentNucleus = nucleus; + Rebuild(inspectorContainer); + } + + void Rebuild(VisualElement inspectorContainer) { + BuildLayers(); + + if (this.currentNucleus == null) { + inspectorContainer.Clear(); + return; + } + + if (currentWrapper != null) + DestroyImmediate(currentWrapper); + currentWrapper = CreateInstance().Init(this.currentNucleus, brain); + DrawInspector(inspectorContainer); + } + + private void BuildLayers() { + // A temporary list to track what's been added to layers + this.layers = new(); + int layerIx = 0; + + INucleus selectedNucleus = this.currentNucleus; + if (selectedNucleus == null) + return; + NeuroidLayer currentLayer = new() { ix = layerIx }; + + if (selectedNucleus.receivers != null) { + foreach (INucleus receiver in selectedNucleus.receivers) { + INucleus outputNeuroid = receiver; + if (outputNeuroid != null) { + AddToLayer(currentLayer, outputNeuroid); + // Debug.Log($"layer {layerIx} nucleus {outputNeuroid.name}"); + } + } + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + layerIx++; + currentLayer = new() { ix = layerIx }; + } + + AddToLayer(currentLayer, selectedNucleus); + this.layers.Add(currentLayer); + // Debug.Log($"layer {layerIx} nucleus {selectedNucleus.name}"); + + layerIx++; + currentLayer = new() { ix = layerIx }; + + if (selectedNucleus.synapses != null) { + foreach (Synapse synapse in selectedNucleus.synapses) { + IReceptor input = synapse.nucleus; + AddToLayer(currentLayer, input); + // Debug.Log($"layer {layerIx} nucleus {input.name}"); + } + } + if (currentLayer.neuroids.Count > 0) { + this.layers.Add(currentLayer); + } + } + + private void AddToLayer(NeuroidLayer layer, IReceptor nucleus) { + if (nucleus == null) + return; + layer.neuroids.Add(nucleus); + //nucleus.layerIx = layer.ix; + // Store its position + Vector2Int neuroidPosition = new(layer.ix, layer.neuroids.Count - 1); + neuroidPositions[nucleus] = neuroidPosition; + + } + + void OnMouseDown(MouseDownEvent e) { + if (e.button == 2) { draggingCanvas = true; lastMouse = e.mousePosition; e.StopPropagation(); } + } + void OnMouseMove(MouseMoveEvent e) { + if (draggingCanvas) { + var delta = e.mousePosition - lastMouse; + pan += delta; + //content.style.left = pan.x; + //content.style.top = pan.y; + lastMouse = e.mousePosition; + } + } + void OnMouseUp(MouseUpEvent e) { if (e.button == 2) draggingCanvas = false; } + + void OnIMGUI() { + if (currentNucleus == null) + return; + + if (Application.isPlaying == false) + serializedBrain.Update(); + + Handles.BeginGUI(); + DrawGraph(); + Handles.EndGUI(); + + } + + private void DrawGraph() { + float size = 20; + Vector3 position = new(150, 210, 0); + + DrawReceivers(this.currentNucleus, position, size); + DrawSynapses(this.currentNucleus, position, size); + + // Draw selected Nucleus + Handles.color = Color.white; + Handles.DrawSolidDisc(position, Vector3.forward, size + 2); + DrawNucleus(this.currentNucleus, position, this.currentNucleus.outputValue.magnitude, 20); + } + + private void DrawReceivers(INucleus nucleus, Vector3 parentPos, float size) { + int nodeCount = nucleus.receivers.Count; + + // Determine the maximum value in this layer + // This is used to 'scale' the output value colors of the nuclei + float maxValue = 0; + foreach (INucleus receiver in nucleus.receivers) { + if (receiver is Neuroid neuroid) { + float value = neuroid.outputValue.magnitude; + if (value > maxValue) + maxValue = value; + } + } + + // Determine the spacing of the nuclei in the layer + float spacing = 400f / nodeCount; + float margin = 10 + spacing / 2; + + int row = 0; + foreach (INucleus receiver in nucleus.receivers) { + INucleus receiverNucleus = receiver; + if (receiverNucleus == null) + continue; + + Vector3 pos = new(50, margin + row * spacing, 0.0f); + Handles.color = Color.white; + Handles.DrawLine(parentPos, pos); + + DrawNucleus(receiverNucleus, pos, maxValue, size); + row++; + } + } + + private void DrawSynapses(INucleus nucleus, Vector3 parentPos, float size) { + int nodeCount = nucleus.synapses.Count; + + // Determine the maximum value in this layer + // This is used to 'scale' the output value colors of the nuclei + float maxValue = 0; + foreach (Synapse receiver in nucleus.synapses) { + if (receiver.nucleus is Neuroid neuroid) { + float value = neuroid.outputValue.magnitude; + if (value > maxValue) + maxValue = value; + } + } + + // Determine the spacing of the nuclei in the layer + float spacing = 400f / nodeCount; + float margin = 10 + spacing / 2; + + int row = 0; + List drawnArrays = new(); + foreach (Synapse synapse in nucleus.synapses) { + Vector3 pos = new(250, margin + row * spacing, 0.0f); + Handles.color = Color.white; + Handles.DrawLine(parentPos, pos); + // if (synapse.nucleus is Perceptoid perceptoid && perceptoid.array != null) { + // // if (drawnArrays.Contains(perceptoid.array)) + // // // We already drawn this array + // // continue; + + // drawnArrays.Add(perceptoid.array); + // DrawArray(perceptoid.array, pos, size); + // } + // else { + + DrawNucleus(synapse.nucleus, pos, maxValue, size); + row++; + // } + } + } + + private void DrawNucleus(IReceptor nucleus, Vector3 position, float maxValue, float size) { + if (nucleus.isSleeping) + Handles.color = Color.darkRed; + else { + if (Application.isPlaying) { + float brightness = nucleus.outputValue.magnitude / maxValue; + Handles.color = new Color(brightness, brightness, brightness, 1f); + } + else + Handles.color = Color.black; + } + Handles.DrawSolidDisc(position, Vector3.forward, size); + + Handles.color = Color.white; + // Position the label in front of the disc + Vector3 labelPosition = position + (Vector3.forward * 0.1f); + + GUIStyle style = new(EditorStyles.label) { + alignment = TextAnchor.MiddleCenter, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold, + }; + if (nucleus is Perceptoid perceptoid) { + if (perceptoid.array == null || perceptoid.array.perceptei == null || perceptoid.array.perceptei.Length == 0) + perceptoid.array = new PercepteiArray(perceptoid); + + if (perceptoid.array.perceptei.Length > 1) { + Handles.Label(labelPosition, perceptoid.array.perceptei.Length.ToString(), style); + } + } + + style.alignment = TextAnchor.UpperCenter; + Vector3 labelPos = position - Vector3.down * (size + 0.2f); // below disc along up axis + Handles.Label(labelPos, nucleus.name, style); + + Rect neuronRect = new(position.x - size, position.y - size, size * 2, size * 2); + int id = GUIUtility.GetControlID(FocusType.Passive); + Event e = Event.current; + EventType et = e.GetTypeForControl(id); + if (e != null && neuronRect.Contains(e.mousePosition)) { + // Process Hover + HandleMouseHover(nucleus, neuronRect); + // Process click + if (e.type == EventType.MouseDown && e.button == 0) { + // Consume the event so the scene doesn't also handle it + e.Use(); + HandleClicked(nucleus); + } + } + } + + private void DrawArray(PercepteiArray array, Vector3 position, float size) { + Vector3 offset = new(size / 4, size / 4, 0); + Handles.color = Color.black; + Handles.DrawSolidDisc(position, Vector3.forward, size); + + GUIStyle style = new(EditorStyles.label) { + alignment = TextAnchor.UpperCenter, + normal = { textColor = Color.white }, + fontStyle = FontStyle.Bold + }; + Handles.Label(position, array.perceptei.Length.ToString(), style); + Vector3 labelPos = position - Vector3.down * (size + 0.2f); // below disc along up axis + Handles.Label(labelPos, array.name, style); + + // To do: add HandleClick (see above) to expand the array + } + + private void HandleMouseHover(IReceptor nucleus, Rect rect) { + GUIContent tooltip; + if (nucleus is Perceptoid perceptoid) { + if (perceptoid.receptor != null) { + tooltip = new( + $"{perceptoid.name}" + + // $"\nType {perceptoid.receptor.thingType}" + + $" Thing {perceptoid.thingId}" + + $"\nValue: {nucleus.outputValue}"); + } + else { + tooltip = new( + $"{perceptoid.name}" + + $"\nThing {perceptoid.thingId}" + + $"\nValue: {nucleus.outputValue}"); + } + } + else if (nucleus is INucleus n) { + tooltip = new( + $"{nucleus.name}" + + $"\nsynapse count {n.synapses.Count}" + + $"\nValue: {nucleus.outputValue}"); + } + else { + tooltip = new( + $"{nucleus.name}" + + $"\nValue: {nucleus.outputValue}"); + } + + Vector2 mousePosition = Event.current.mousePosition; + + // Display tooltip with some offset + Vector2 tooltipSize = GUI.skin.box.CalcSize(tooltip); + Rect tooltipRect = new Rect(mousePosition.x + 10, mousePosition.y + 10, tooltipSize.x, tooltipSize.y); + + GUI.Box(tooltipRect, tooltip); + } + + private void HandleClicked(IReceptor nucleus) { + if (nucleus is INucleus n) { + this.currentNucleus = n; + BuildLayers(); + } + } + + void DrawInspector(VisualElement inspectorContainer) { + if (inspectorContainer == null) + return; + + inspectorContainer.Clear(); + if (this.currentNucleus == null) + return; + + // create a SerializedObject wrapper so Unity inspector controls work (and Undo) + SerializedObject so = new(currentWrapper); + IMGUIContainer container = new(() => { + if (so.targetObject == null) + return; + so.Update(); + + if (this.currentNucleus == null) + return; + + this.currentNucleus.name = EditorGUILayout.TextField(this.currentNucleus.name); + if (this.currentNucleus is Perceptoid perceptoid) { + // perceptoid.receptor.thingType = EditorGUILayout.IntField("Thing Type", perceptoid.receptor.thingType); + + if (perceptoid.array == null || perceptoid.array.perceptei == null || perceptoid.array.perceptei.Length == 0) + perceptoid.array = new PercepteiArray(perceptoid); + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.IntField("Array size", perceptoid.array.perceptei.Length); + if (GUILayout.Button("Add")) + perceptoid.array.AddPerceptoid(); + if (GUILayout.Button("Del")) + perceptoid.array.RemovePerceptoid(); + EditorGUILayout.EndHorizontal(); + } + else if (this.currentNucleus is Neuroid neuroid) { + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField("Activation Curve", GUILayout.Width(150)); + if (neuroid.curveMax > 0) + EditorGUILayout.CurveField(neuroid.curve, Color.cyan, new Rect(0, 0, 1, neuroid.curveMax)); + else + EditorGUILayout.CurveField(neuroid.curve, Color.cyan, new Rect(0, neuroid.curveMax, 1, -neuroid.curveMax)); + neuroid.curvePreset = (Neuroid.CurvePresets)EditorGUILayout.EnumPopup(neuroid.curvePreset, GUILayout.Width(100)); + EditorGUILayout.EndHorizontal(); + } + + if (Application.isPlaying) + EditorGUILayout.FloatField("Output", this.currentNucleus.outputValue.magnitude); + else + EditorGUILayout.LabelField(" "); + + if (this.currentNucleus.synapses.Count > 0) { + Synapse[] synapses = this.currentNucleus.synapses.ToArray(); + foreach (Synapse synapse in synapses) { + if (synapse.nucleus != null) { + EditorGUILayout.Space(); + + EditorGUI.BeginDisabledGroup(synapse.nucleus.isSleeping); + if (Application.isPlaying) + EditorGUILayout.FloatField(synapse.nucleus.name, synapse.nucleus.outputValue.magnitude * synapse.weight); + else { + EditorGUILayout.BeginHorizontal(); + EditorGUILayout.LabelField(synapse.nucleus.name); + // if (synapse.nucleus is Perceptoid perceptoid) { + // if (perceptoid.array == null || perceptoid.array.perceptei == null || perceptoid.array.perceptei.Length == 0) { + // perceptoid.array = new PercepteiArray(perceptoid); + // } + // EditorGUILayout.IntField(perceptoid.array.perceptei.Length); + // if (GUILayout.Button("Add")) + // perceptoid.array.AddPerceptoid(); + // } + if (GUILayout.Button("Disconnect")) + synapse.nucleus.RemoveReceiver(this.currentNucleus); + EditorGUILayout.EndHorizontal(); + } + + EditorGUI.indentLevel++; + synapse.weight = EditorGUILayout.FloatField("Weight", synapse.weight); + EditorGUI.indentLevel--; + EditorGUI.EndDisabledGroup(); + } + } + } + + EditorGUILayout.Space(); + + ConnectNucleus(this.currentNucleus); + if (GUILayout.Button("Add Input Neuron")) + AddInputNeuron(this.currentNucleus); + if (GUILayout.Button("Add Input Perceptoid")) + AddPerceptoid(this.currentNucleus); + if (GUILayout.Button("Add Input Cluster")) + AddCluster(this.currentNucleus); + + EditorGUILayout.Space(); + + if (GUILayout.Button("Delete this neuron")) + DeleteNeuron(this.currentNucleus); + + //DisconnectNucleus(this.currentNucleus); + + if (this.gameObject != null) { + Vector3 worldVector = this.gameObject.transform.TransformVector(this.currentNucleus.outputValue); + Debug.DrawRay(this.gameObject.transform.position, worldVector, Color.yellow); + } + }); + + inspectorContainer.Add(container); + } + + protected virtual void AddInputNeuron(INucleus nucleus) { + Neuroid newNeuroid = new(this.brain.cluster, "New neuron"); + newNeuroid.AddReceiver(nucleus); + this.currentNucleus = newNeuroid; + BuildLayers(); + } + + protected virtual void DeleteNeuron(INucleus nucleus) { + if (nucleus == null) + return; + if (nucleus.cluster != null) + this.currentNucleus = nucleus.cluster.output; + foreach (INucleus receiver in nucleus.receivers) { + if (receiver != null) { + this.currentNucleus = receiver; + break; + } + } + Nucleus.Delete(nucleus); + BuildLayers(); + } + + protected virtual void AddPerceptoid(INucleus nucleus) { + Perceptoid newPerceptoid = new(this.brain, 0, "New Perceptoid"); + newPerceptoid.AddReceiver(nucleus); + this.currentNucleus = newPerceptoid; + BuildLayers(); + } + + protected virtual void AddCluster(INucleus nucleus) { + BrainPickerWindow.ShowPicker(brain => OnClusterPicked(nucleus, brain), "Select Cluster"); + } + + private void OnClusterPicked(INucleus nucleus, NanoBrain brain) { + NanoBrain brainInstance = Instantiate(brain); + brainInstance.AddReceiver(nucleus); + } + + protected virtual void ConnectNucleus(INucleus nucleus) { + if (this.currentNucleus.cluster == null) + return; + + IEnumerable synapseNuclei = this.currentNucleus.synapses.Select(synapse => synapse.nucleus.name); + //IEnumerable perceptei = this.currentNucleus.brain.perceptei.Select(i => i.name).Except(synapseNuclei); + IEnumerable nuclei = this.currentNucleus.cluster.nuclei.Select(i => i.name).Except(synapseNuclei); + //string[] names = perceptei.Concat(nuclei).ToArray(); + string[] names = nuclei.ToArray(); + int selectedIndex = -1; + selectedIndex = EditorGUILayout.Popup("Connect to", selectedIndex, names); + if (selectedIndex >= 0) { + // if (selectedIndex < perceptei.Count()) { + // Nucleus n = this.currentNucleus.brain.perceptei[selectedIndex]; + // n.AddReceiver(this.currentNucleus); + // } + // else { + // Nucleus n = this.currentNucleus.brain.nuclei[selectedIndex - perceptei.Count()]; + // n.AddReceiver(this.currentNucleus); + // } + INucleus n = this.currentNucleus.cluster.nuclei[selectedIndex]; + n.AddReceiver(this.currentNucleus); + } + } + + protected virtual void DisconnectNucleus(Nucleus nucleus) { + if (this.currentNucleus.cluster == null) + return; + string[] names = this.currentNucleus.synapses.Select(synapse => synapse.nucleus.name).ToArray(); + int selectedIndex = -1; + selectedIndex = EditorGUILayout.Popup("Disconnect from", selectedIndex, names); + //if (selectedIndex >= 0 && selectedIndex < this.currentNucleus.brain.perceptei.Count) { + if (selectedIndex >= 0 && selectedIndex < this.currentNucleus.cluster.nuclei.Count) { + Synapse synapse = this.currentNucleus.synapses[selectedIndex]; + synapse.nucleus.RemoveReceiver(this.currentNucleus); + } + } + } + + #endregion Start + + #region Update + + private void UpdateLayout(float containerWidth) { + if (containerWidth > 600f) { + mainContainer.style.flexDirection = FlexDirection.Row; + inspectorContainer.style.width = 300; // fixed sidebar width + inspectorContainer.style.flexGrow = 0; + } + else { + mainContainer.style.flexDirection = FlexDirection.Column; + inspectorContainer.style.width = Length.Percent(100); // full width below + inspectorContainer.style.flexDirection = FlexDirection.Column; + inspectorContainer.style.flexGrow = 1; // can set 0 or keep as needed + } + } + + #endregion Update +} + +/* +public class NeuroidLayer { + public int ix = 0; + public List neuroids = new(); +} +*/ + +/* +public class GraphNodeWrapper : ScriptableObject { + // expose fields that map to GraphNode + //public string title; + public Vector2 position; + INucleus node; + NanoBrain graph; // needed to write back and mark dirty + + public GraphNodeWrapper Init(INucleus node, NanoBrain graphAsset) { + this.node = node; + this.graph = graphAsset; + //this.title = " A " + node.name; + //position = node.position; + return this; + } + void OnValidate() { + if (node != null) { + //node.name = title; + //node.position = position; +#if UNITY_EDITOR + if (graph != null) + UnityEditor.EditorUtility.SetDirty(graph); +#endif + } + } +} +*/ \ No newline at end of file diff --git a/VisualEditor/Editor/NanoBrainInspector.cs.meta b/VisualEditor/Editor/NanoBrainInspector.cs.meta new file mode 100644 index 0000000..e71178e --- /dev/null +++ b/VisualEditor/Editor/NanoBrainInspector.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: c96ad47c3d4498640b52630789e38573 \ No newline at end of file diff --git a/VisualEditor/NanoBrain.cs b/VisualEditor/NanoBrain.cs new file mode 100644 index 0000000..d786da6 --- /dev/null +++ b/VisualEditor/NanoBrain.cs @@ -0,0 +1,102 @@ +/* +using System; +using System.Collections.Generic; +using UnityEngine; + +[CreateAssetMenu(menuName = "Passer/NanoBrain")] +public class NanoBrain : ScriptableObject, ISerializationCallbackReceiver { + public List nuclei = new(); + public List perceptei = new(); + public List receptors = new(); + + + // This is probably always the first element in the nuclei list... + [System.NonSerialized] + public Nucleus output; + public int rootId; + + public NanoBrain() { + // this.cluster = new(); + // this.output = new Neuroid(this.cluster, "Root"); + } + + public Cluster cluster; + + public void AddReceiver(INucleus receiver) { + output.AddReceiver(receiver); + } + + public Neuroid AddNeuron(string name) { + Neuroid neuroid = new(this.cluster, name); + return neuroid; + } + + public void UpdateNuclei() { + foreach (Nucleus nucleus in nuclei) + nucleus.IncreaseAge(); + foreach (Perceptoid perception in perceptei) + perception.IncreaseAge(); + } + + public void OnBeforeSerialize() { + if (output != null) { + this.rootId = output.id; + } + } + public void OnAfterDeserialize() { + try { + foreach (Nucleus nucleus in this.nuclei.ToArray()) { + if (this.rootId == nucleus.id) + this.output = nucleus; + nucleus.Rebuild(this); + } + + foreach (Perceptoid perceptoid in this.perceptei.ToArray()) + perceptoid.Rebuild(this); + } + catch (System.Exception) { } + if (this.cluster != null) + this.cluster.GarbageCollection(); + } + + + public void GarbageCollection() { + HashSet visitedNuclei = new(); + MarkNuclei(visitedNuclei, this.output); + //Debug.Log($"Garbage collection found {visitedNuclei.Count} Nuclei"); + this.nuclei.RemoveAll(nucleus => visitedNuclei.Contains(nucleus) == false); + this.perceptei.RemoveAll(perceptoid => visitedNuclei.Contains(perceptoid) == false); + } + + public void MarkNuclei(HashSet visitedNuclei, INucleus nucleus) { + if (nucleus is null) + return; + + if (nucleus.brain == null) + nucleus.brain = this; + + visitedNuclei.Add(nucleus); + if (nucleus.synapses != null) { + HashSet visitedSynapses = new(); + foreach (Synapse synapse in nucleus.synapses) { + if (synapse != null && synapse.nucleus != null) { + visitedSynapses.Add(synapse); + MarkNuclei(visitedNuclei, synapse.nucleus); + } + } + nucleus.synapses.RemoveAll(synapse => visitedSynapses.Contains(synapse) == false); + } + if (nucleus.receivers != null) { + HashSet visitedReceivers = new(); + foreach (Receiver receiver in nucleus.receivers) { + if (receiver != null && receiver.nucleus != null) { + visitedReceivers.Add(receiver); + visitedNuclei.Add(receiver.nucleus); + } + } + nucleus.receivers.RemoveAll(receiver => visitedReceivers.Contains(receiver) == false); + } + } + +} +*/ \ No newline at end of file diff --git a/VisualEditor/NanoBrain.cs.meta b/VisualEditor/NanoBrain.cs.meta new file mode 100644 index 0000000..40e85f2 --- /dev/null +++ b/VisualEditor/NanoBrain.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 36081359186edfec998d891a1feeb17b \ No newline at end of file diff --git a/VisualEditor/NanoBrainComponent.cs b/VisualEditor/NanoBrainComponent.cs new file mode 100644 index 0000000..56c9159 --- /dev/null +++ b/VisualEditor/NanoBrainComponent.cs @@ -0,0 +1,35 @@ +using UnityEngine; + +public class NanoBrainComponent : MonoBehaviour { + public ClusterPrefab defaultBrain; + private ClusterPrefab brainInstance; + + public INucleus root => brainInstance.output; + public ClusterPrefab brain { + get { + if (brainInstance == null && defaultBrain != null) { + brainInstance = Instantiate(defaultBrain); + brainInstance.name = defaultBrain.name + " (Instance)"; + + SwarmControl sc = FindFirstObjectByType(); + if (sc != null) { + UpdateWeight(brainInstance, "Avoidance", sc.avoidanceForce); + UpdateWeight(brainInstance, "Cohesion", sc.cohesionForce); + UpdateWeight(brainInstance, "Separation", sc.separationForce); + UpdateWeight(brainInstance, "Alignment", sc.alignmentForce); + } + } + return brainInstance; + } + } + + public static void UpdateWeight(ClusterPrefab brain, string name, float weight) { + INucleus root = brain.output; + foreach (Synapse synapse in root.synapses) { + if (synapse.nucleus.name == name) { + synapse.weight = weight; + } + } + } + +} \ No newline at end of file diff --git a/VisualEditor/NanoBrainComponent.cs.meta b/VisualEditor/NanoBrainComponent.cs.meta new file mode 100644 index 0000000..1666c60 --- /dev/null +++ b/VisualEditor/NanoBrainComponent.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 92f34a5e4027a1dc39efd8ce63cf6aba \ No newline at end of file diff --git a/VisualEditor/Resources.meta b/VisualEditor/Resources.meta new file mode 100644 index 0000000..e9c19e4 --- /dev/null +++ b/VisualEditor/Resources.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7b61a93fc9332d2adae74fe4abe92d53 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/VisualEditor/Resources/GraphStyles.uss b/VisualEditor/Resources/GraphStyles.uss new file mode 100644 index 0000000..79bafe8 --- /dev/null +++ b/VisualEditor/Resources/GraphStyles.uss @@ -0,0 +1,12 @@ +#content { + background-color: #2b2b2b; +} +#inspector { + border-left-width: 1px; + border-left-color: #000; + padding: 3px; +} +#title { + -unity-font-style: bold; + color: white; + } diff --git a/VisualEditor/Resources/GraphStyles.uss.meta b/VisualEditor/Resources/GraphStyles.uss.meta new file mode 100644 index 0000000..2546c45 --- /dev/null +++ b/VisualEditor/Resources/GraphStyles.uss.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 28268b644fa8f3948851b25e41f5b03b +ScriptedImporter: + internalIDToNameTable: [] + externalObjects: {} + serializedVersion: 2 + userData: + assetBundleName: + assetBundleVariant: + script: {fileID: 12385, guid: 0000000000000000e000000000000000, type: 0} + disableValidation: 0