Merge commit 'ac376338de4df35266136050d49e37c4c3f03180' into V2
This commit is contained in:
commit
15ac8a399b
@ -3,12 +3,14 @@ using System.Diagnostics;
|
||||
using Passer.LinearAlgebra;
|
||||
#if UNITY_5_3_OR_NEWER
|
||||
using Vector3Float = UnityEngine.Vector3;
|
||||
using Vector2Float = UnityEngine.Vector2;
|
||||
#endif
|
||||
using Quaternion = UnityEngine.Quaternion;
|
||||
|
||||
public readonly struct Slice {
|
||||
public int start { get; }
|
||||
public int stop { get; }
|
||||
public Slice(int start, int stop) {
|
||||
public uint start { get; }
|
||||
public uint stop { get; }
|
||||
public Slice(uint start, uint stop) {
|
||||
this.start = start;
|
||||
this.stop = stop;
|
||||
}
|
||||
@ -27,21 +29,39 @@ public class Matrix2 {
|
||||
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(uint nRows, uint nCols) {
|
||||
return new Matrix2(nRows, nCols);
|
||||
}
|
||||
|
||||
public static Matrix2 FromVector3(Vector3Float v) {
|
||||
float[,] result = new float[3, 1];
|
||||
result[0, 0] = v.x;
|
||||
result[1, 0] = v.y;
|
||||
result[2, 0] = v.z;
|
||||
return new Matrix2(result);
|
||||
}
|
||||
|
||||
public static Matrix2 Identity(uint size) {
|
||||
return Diagonal(1, size);
|
||||
// float[,] resultData = new float[size, size];
|
||||
// for (int i = 0; i < size; i++)
|
||||
// resultData[i, i] = 1.0f;
|
||||
// return new Matrix2(resultData);
|
||||
}
|
||||
public static Matrix2 Identity(uint nRows, uint nCols) {
|
||||
Matrix2 m = Zero(nRows, nCols);
|
||||
m.FillDiagonal(1);
|
||||
return m;
|
||||
}
|
||||
|
||||
public static Matrix2 Diagonal(Matrix1 v) {
|
||||
float[,] resultData = new float[v.magnitude, v.magnitude];
|
||||
for (int ix = 0; ix < v.magnitude; ix++)
|
||||
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);
|
||||
}
|
||||
@ -51,6 +71,16 @@ public class Matrix2 {
|
||||
resultData[ix, ix] = f;
|
||||
return new Matrix2(resultData);
|
||||
}
|
||||
public void FillDiagonal(Matrix1 v) {
|
||||
uint n = 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) {
|
||||
uint 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] {
|
||||
@ -61,6 +91,44 @@ public class Matrix2 {
|
||||
return new Matrix2(result);
|
||||
}
|
||||
|
||||
public Vector3Float GetRow3(int rowIx) {
|
||||
uint cols = this.nCols;
|
||||
Vector3Float row = new() {
|
||||
x = this.data[rowIx, 0],
|
||||
y = this.data[rowIx, 1],
|
||||
z = this.data[rowIx, 2]
|
||||
};
|
||||
return row;
|
||||
}
|
||||
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.x;
|
||||
this.data[rowIx, 1] = v.y;
|
||||
this.data[rowIx, 2] = v.z;
|
||||
}
|
||||
|
||||
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 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++) {
|
||||
@ -70,6 +138,9 @@ public class Matrix2 {
|
||||
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];
|
||||
@ -147,31 +218,51 @@ public class Matrix2 {
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
result[i, j] = A.data[i, j] * s;
|
||||
}
|
||||
|
||||
return new Matrix2(result);
|
||||
}
|
||||
public static Matrix2 operator *(float scalar, Matrix2 A) {
|
||||
return A * scalar;
|
||||
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 Slice(Slice slice) {
|
||||
return Slice(slice.start, slice.stop);
|
||||
}
|
||||
public Matrix2 Slice(int from, int to) {
|
||||
public Matrix2 Slice(uint from, uint 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 (uint rowIx = from; rowIx < to; rowIx++) {
|
||||
for (int colIx = 0; colIx < this.nCols; colIx++) {
|
||||
result[resultRowIx, colIx] = this.data[rowIx, colIx];
|
||||
}
|
||||
@ -180,17 +271,36 @@ public class Matrix2 {
|
||||
|
||||
return new Matrix2(result);
|
||||
}
|
||||
public Matrix2 Slice(Slice rowRange, Slice colRange) {
|
||||
return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
|
||||
}
|
||||
|
||||
public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange) {
|
||||
float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
|
||||
|
||||
uint resultRowIx = 0;
|
||||
uint resultColIx = 0;
|
||||
for (uint i = rowRange.start; i < rowRange.stop; i++) {
|
||||
for (uint 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) {
|
||||
int mRowIx = 0;
|
||||
for (int rowIx = slice.start; rowIx < slice.stop; rowIx++) {
|
||||
for (uint 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) {
|
||||
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.stop];
|
||||
UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
|
||||
}
|
||||
public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m) {
|
||||
for (uint i = rowRange.start; i < rowRange.stop; i++) {
|
||||
for (uint j = colRange.start; j < colRange.stop; j++)
|
||||
this.data[i, j] = m.data[i - rowRange.start, j - colRange.start];
|
||||
}
|
||||
}
|
||||
|
||||
@ -246,23 +356,63 @@ public class Matrix2 {
|
||||
|
||||
return new Matrix2(inverse);
|
||||
}
|
||||
|
||||
public float Determinant() {
|
||||
uint 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) {
|
||||
uint 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 class Matrix1 {
|
||||
public float[] data { get; }
|
||||
|
||||
public uint magnitude => (uint)data.GetLength(0);
|
||||
public uint size => (uint)data.GetLength(0);
|
||||
|
||||
public Matrix1(uint magnitude) {
|
||||
this.data = new float[magnitude];
|
||||
public Matrix1(uint size) {
|
||||
this.data = new float[size];
|
||||
}
|
||||
|
||||
public Matrix1(float[] data) {
|
||||
this.data = data;
|
||||
}
|
||||
|
||||
public static Matrix1 Zero(uint magnitude) {
|
||||
return new Matrix1(magnitude);
|
||||
public static Matrix1 Zero(uint size) {
|
||||
return new Matrix1(size);
|
||||
}
|
||||
|
||||
public static Matrix1 FromVector2(Vector2Float v) {
|
||||
@ -280,44 +430,88 @@ public class Matrix1 {
|
||||
return new Matrix1(result);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
public Vector2Float vector2 {
|
||||
get {
|
||||
if (this.magnitude != 2)
|
||||
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.magnitude != 3)
|
||||
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]);
|
||||
}
|
||||
}
|
||||
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]);
|
||||
}
|
||||
}
|
||||
|
||||
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.magnitude];
|
||||
for (uint colIx = 0; colIx < this.magnitude; colIx++)
|
||||
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.magnitude != b.magnitude)
|
||||
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.magnitude; i++) {
|
||||
for (int i = 0; i < a.size; i++) {
|
||||
result += a.data[i] * b.data[i];
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public static Matrix1 operator *(Matrix1 A, float f) {
|
||||
float[] result = new float[A.magnitude];
|
||||
float[] result = new float[A.size];
|
||||
|
||||
for (int i = 0; i < A.magnitude; i++)
|
||||
for (int i = 0; i < A.size; i++)
|
||||
result[i] += A.data[i] * f;
|
||||
|
||||
return new Matrix1(result);
|
||||
@ -329,197 +523,20 @@ public class Matrix1 {
|
||||
public Matrix1 Slice(Slice range) {
|
||||
return Slice(range.start, range.stop);
|
||||
}
|
||||
public Matrix1 Slice(int from, int to) {
|
||||
if (from < 0 || to >= this.magnitude)
|
||||
public Matrix1 Slice(uint from, uint 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++)
|
||||
for (uint 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++)
|
||||
for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
|
||||
this.data[ix] = v.data[vIx];
|
||||
}
|
||||
}
|
||||
|
||||
// public class Matrix {
|
||||
// private readonly uint rows = 0;
|
||||
// private readonly uint cols = 0;
|
||||
// private float[] data;
|
||||
|
||||
// public Matrix(uint rows, uint cols) {
|
||||
// this.rows = rows;
|
||||
// this.cols = cols;
|
||||
// }
|
||||
|
||||
// public static float[,] Diagonal(float[] v) {
|
||||
// float[,] r = new float[v.Length, v.Length];
|
||||
// for (int i = 0; i < v.Length; i++) {
|
||||
// r[i, i] = v[i];
|
||||
// }
|
||||
// return r;
|
||||
// }
|
||||
|
||||
// public static float[,] Transpose(float[,] m) {
|
||||
// int rows = m.GetLength(0);
|
||||
// int cols = m.GetLength(1);
|
||||
// float[,] r = new float[cols, rows];
|
||||
// for (uint rowIx = 0; rowIx < rows; rowIx++) {
|
||||
// for (uint colIx = 0; colIx < cols; colIx++)
|
||||
// r[colIx, rowIx] = m[rowIx, colIx];
|
||||
// }
|
||||
// return r;
|
||||
// // double checked code
|
||||
// }
|
||||
|
||||
// public static void NegateColumn(float[,] m, uint colIx) {
|
||||
// for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) {
|
||||
// m[rowIx, colIx] = -m[rowIx, colIx];
|
||||
// }
|
||||
// }
|
||||
|
||||
// public static float Determinant(float[,] matrix) {
|
||||
// int n = matrix.GetLength(0);
|
||||
// if (n != matrix.GetLength(1))
|
||||
// throw new System.ArgumentException("Matrix must be square.");
|
||||
|
||||
// if (n == 1)
|
||||
// return matrix[0, 0]; // Base case for 1x1 matrix
|
||||
|
||||
// if (n == 2) // Base case for 2x2 matrix
|
||||
// return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0];
|
||||
|
||||
// float det = 0;
|
||||
// for (int col = 0; col < n; col++)
|
||||
// det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col));
|
||||
|
||||
// return det;
|
||||
// }
|
||||
|
||||
// // Helper function to compute the minor of a matrix
|
||||
// private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) {
|
||||
// int n = matrix.GetLength(0);
|
||||
// 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] = matrix[i, j];
|
||||
// c++;
|
||||
// }
|
||||
// r++;
|
||||
// }
|
||||
|
||||
// return minor;
|
||||
// }
|
||||
|
||||
// public static float[,] MultiplyMatrices(float[,] A, float[,] B) {
|
||||
// int rowsA = A.GetLength(0);
|
||||
// int colsA = A.GetLength(1);
|
||||
// int rowsB = B.GetLength(0);
|
||||
// int colsB = B.GetLength(1);
|
||||
|
||||
// if (colsA != rowsB)
|
||||
// throw new System.ArgumentException("Number of columns in A must match number of rows in B.");
|
||||
|
||||
// float[,] result = new float[rowsA, colsB];
|
||||
|
||||
// for (int i = 0; i < rowsA; i++) {
|
||||
// for (int j = 0; j < colsB; j++) {
|
||||
// float sum = 0.0f;
|
||||
// for (int k = 0; k < colsA; k++)
|
||||
// sum += A[i, k] * B[k, j];
|
||||
|
||||
// result[i, j] = sum;
|
||||
// }
|
||||
// }
|
||||
|
||||
// return result;
|
||||
// // double checked code
|
||||
// }
|
||||
|
||||
// public static float[] MultiplyMatrixVector(float[,] A, float[] v) {
|
||||
// int rows = A.GetLength(0);
|
||||
// int cols = A.GetLength(1);
|
||||
// float[] result = new float[rows];
|
||||
|
||||
// for (int i = 0; i < rows; i++) {
|
||||
// for (int j = 0; j < cols; j++) {
|
||||
// result[i] += A[i, j] * v[j];
|
||||
// }
|
||||
// }
|
||||
|
||||
// return result;
|
||||
// }
|
||||
|
||||
// // Vector-matrix multiplication
|
||||
// public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) {
|
||||
// return new Vector3Float(
|
||||
// A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z,
|
||||
// A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z,
|
||||
// A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z
|
||||
// );
|
||||
// }
|
||||
|
||||
// public static float[,] MultiplyMatrixScalar(float[,] A, float s) {
|
||||
// int rows = A.GetLength(0);
|
||||
// int cols = A.GetLength(1);
|
||||
// float[,] result = new float[rows, cols];
|
||||
|
||||
// for (int i = 0; i < rows; i++) {
|
||||
// for (int j = 0; j < cols; j++) {
|
||||
// result[i, j] += A[i, j] * s;
|
||||
// }
|
||||
// }
|
||||
|
||||
// return result;
|
||||
// }
|
||||
|
||||
// public static float[] GetColumn(float[,] M, int col) {
|
||||
// int rows = M.GetLength(0);
|
||||
// float[] column = new float[rows];
|
||||
// for (int i = 0; i < rows; i++) {
|
||||
// column[i] = M[i, col];
|
||||
// }
|
||||
// return column;
|
||||
// }
|
||||
|
||||
// public static Vector3Float GetRow3(float[,] M, int rowIx) {
|
||||
// int cols = M.GetLength(1);
|
||||
// Vector3Float row = new(
|
||||
// M[rowIx, 0],
|
||||
// M[rowIx, 1],
|
||||
// M[rowIx, 2]
|
||||
// );
|
||||
// return row;
|
||||
// }
|
||||
|
||||
// public static void SetRow3(float[,] M, int rowIx, Vector3Float v) {
|
||||
// M[rowIx, 0] = v.x;
|
||||
// M[rowIx, 1] = v.y;
|
||||
// M[rowIx, 2] = v.z;
|
||||
// }
|
||||
|
||||
// public static float Dot(float[] a, float[] b) {
|
||||
// float sum = 0;
|
||||
// for (int i = 0; i < a.Length; i++) sum += a[i] * b[i];
|
||||
// return sum;
|
||||
// }
|
||||
|
||||
// public static float[,] IdentityMatrix(int size) {
|
||||
// float[,] I = new float[size, size];
|
||||
// for (int i = 0; i < size; i++) I[i, i] = 1.0f;
|
||||
// return I;
|
||||
// }
|
||||
|
||||
// }
|
@ -1,3 +1,5 @@
|
||||
using System;
|
||||
using Quaternion = UnityEngine.Quaternion;
|
||||
namespace Passer.LinearAlgebra {
|
||||
|
||||
public class QuaternionOf<T> {
|
||||
@ -13,19 +15,58 @@ namespace Passer.LinearAlgebra {
|
||||
this.w = w;
|
||||
}
|
||||
|
||||
public static float[,] ToRotationMatrix(Quaternion q) {
|
||||
public static Matrix2 ToRotationMatrix(Quaternion q) {
|
||||
float w = q.x, x = q.y, y = q.z, z = q.w;
|
||||
|
||||
return new float[,]
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
public class Quaternion : QuaternionOf<float> {
|
||||
public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
|
||||
return new Quaternion(x, y, z, w);
|
||||
}
|
||||
}
|
||||
|
||||
// public class Quaternion : QuaternionOf<float> {
|
||||
// public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
|
||||
// }
|
||||
}
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A message containing binary data for custom communication
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message notifiying that a Thing no longer exists
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message to request details for a Thing
|
||||
|
@ -1,6 +1,6 @@
|
||||
using Passer.LinearAlgebra;
|
||||
|
||||
namespace Passer.RoboidControl
|
||||
namespace RoboidControl
|
||||
{
|
||||
public class LowLevelMessages
|
||||
{
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Root structure for all communcation messages
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message for communicating the URL for a model of the thing
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message for communicating the name of a thing
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A message communicating the network ID for that participant
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A participant messages notifies other participants of its presence
|
||||
|
@ -1,6 +1,6 @@
|
||||
using Passer.LinearAlgebra;
|
||||
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message to communicate the pose of the thing
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message for sending generic text
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// Message providing generic information about a Thing
|
||||
|
@ -4,7 +4,7 @@ using System.Collections.Concurrent;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A participant is used for communcation between things
|
||||
@ -303,7 +303,10 @@ namespace Passer.RoboidControl {
|
||||
}
|
||||
|
||||
protected virtual void Process(RemoteParticipant sender, ModelUrlMsg msg) {
|
||||
Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
|
||||
//Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
|
||||
Thing thing = sender.Get(msg.networkId, msg.thingId);
|
||||
if (thing != null)
|
||||
thing.modelUrl = msg.url;
|
||||
Thing thing = sender.Get(msg.networkId, msg.thingId);
|
||||
if (thing != null)
|
||||
thing.modelUrl = msg.url;
|
||||
|
@ -1,7 +1,7 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A reference to a participant, possibly on a remote location
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A sensor measuring the distance in the forward direction
|
||||
|
@ -1,6 +1,6 @@
|
||||
//using System;
|
||||
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A temperature sensor
|
||||
|
@ -1,4 +1,4 @@
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A sensor which can detect touches
|
||||
|
@ -2,7 +2,7 @@ using System;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A site server is a participant which provides a shared simulated environment
|
||||
@ -52,7 +52,7 @@ namespace Passer.RoboidControl {
|
||||
protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { }
|
||||
|
||||
protected override void Process(RemoteParticipant sender, ThingMsg msg) {
|
||||
Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
|
||||
// Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
|
||||
Thing thing = sender.Get(msg.networkId, msg.thingId);
|
||||
if (thing == null) {
|
||||
Thing newThing = null;
|
||||
@ -63,7 +63,7 @@ namespace Passer.RoboidControl {
|
||||
}
|
||||
if (newThing == null) {
|
||||
newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
|
||||
Console.WriteLine("Created generic new core thing");
|
||||
// Console.WriteLine("Created generic new core thing");
|
||||
}
|
||||
if (msg.parentId != 0) {
|
||||
Thing parentThing = Get(msg.networkId, msg.parentId);
|
||||
|
2
Thing.cs
2
Thing.cs
@ -2,7 +2,7 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using Passer.LinearAlgebra;
|
||||
|
||||
namespace Passer.RoboidControl {
|
||||
namespace RoboidControl {
|
||||
|
||||
/// <summary>
|
||||
/// A thing is the primitive building block
|
||||
|
@ -3,7 +3,7 @@ using System.IO;
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace Passer.RoboidControl.Unity {
|
||||
namespace RoboidControl.Unity {
|
||||
|
||||
public class UnityLogWriter : TextWriter {
|
||||
public override void Write(char value) {
|
||||
|
@ -2,7 +2,7 @@
|
||||
using System.Collections;
|
||||
using UnityEngine;
|
||||
|
||||
namespace Passer.RoboidControl.Unity {
|
||||
namespace RoboidControl.Unity {
|
||||
|
||||
/// <summary>
|
||||
/// The Unity representation of a distance sensor
|
||||
|
@ -3,7 +3,7 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using UnityEngine;
|
||||
|
||||
namespace Passer.RoboidControl.Unity {
|
||||
namespace RoboidControl.Unity {
|
||||
|
||||
public class SiteServer : MonoBehaviour {
|
||||
public RoboidControl.SiteServer site;
|
||||
|
@ -3,7 +3,7 @@ using System.Collections;
|
||||
using UnityEngine;
|
||||
using UnityEngine.Networking;
|
||||
|
||||
namespace Passer.RoboidControl.Unity {
|
||||
namespace RoboidControl.Unity {
|
||||
|
||||
/// <summary>
|
||||
/// The representation of a Thing in Unity
|
||||
@ -35,7 +35,7 @@ namespace Passer.RoboidControl.Unity {
|
||||
}
|
||||
|
||||
public static Thing Create(RoboidControl.Thing core) {
|
||||
Debug.Log("Creating new Unity thing");
|
||||
// Debug.Log("Creating new Unity thing");
|
||||
GameObject gameObj = string.IsNullOrEmpty(core.name) ?
|
||||
new("Thing") :
|
||||
new(core.name);
|
||||
|
@ -1,7 +1,7 @@
|
||||
#if UNITY_5_3_OR_NEWER
|
||||
using UnityEngine;
|
||||
|
||||
namespace Passer.RoboidControl.Unity {
|
||||
namespace RoboidControl.Unity {
|
||||
|
||||
/// <summary>
|
||||
/// The Unity representation of the TouchSensor
|
||||
|
Loading…
x
Reference in New Issue
Block a user