From 4c99d67232f2f65391c30bfaed8b00763abde4b3 Mon Sep 17 00:00:00 2001
From: villberg <villberg@ac1ea38d-2e2b-0410-8846-a27921b304fc>
Date: Fri, 26 Sep 2014 12:56:14 +0000
Subject: [PATCH] Ones was broken, transpose was missing, multiplication
 operator did not handle all cases. Better error reporting. Detection and
 reporting of infinities. refs #5224

git-svn-id: https://www.simantics.org/svn/simantics/sysdyn/trunk@30323 ac1ea38d-2e2b-0410-8846-a27921b304fc
---
 .../src/fi/semantum/sysdyn/solver/Array.java  | 61 ++++++++++++++++++-
 .../semantum/sysdyn/solver/Environment.java   | 13 +++-
 .../sysdyn/solver/Multiplication.java         |  4 ++
 .../src/fi/semantum/sysdyn/solver/Solver.java |  7 ++-
 4 files changed, 82 insertions(+), 3 deletions(-)

diff --git a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Array.java b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Array.java
index e7b1959d..6cdc5b83 100644
--- a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Array.java
+++ b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Array.java
@@ -16,6 +16,9 @@ import java.util.Iterator;
 import java.util.List;
 import java.util.Map;
 
+/*
+ * For matrices first dimension is rows and second is columns i.e. top level elements are rows
+ */
 public class Array implements IExpression {
 
 	public static final Array FULL = new Array();
@@ -309,7 +312,7 @@ public class Array implements IExpression {
 		double result = 0;
 		Collection<Object> lae = elements();
 		Collection<Object> rae = other.elements();
-		if(lae.size() != rae.size()) throw new IllegalStateException();
+		if(lae.size() != rae.size()) throw new IllegalStateException("inner product: vector sizes do not match: " + this + " vs. " + other);
 		Iterator<Object> li = lae.iterator();
 		Iterator<Object> ri = rae.iterator();
 		for(int i=0;i<lae.size();i++) {
@@ -324,6 +327,37 @@ public class Array implements IExpression {
 		return result;
 	}
 
+	/*
+	 * Other array is vector (this is matrix)
+	 */
+	public Array matrixMulVector(Array other) {
+		Array result = new Array();
+		for(Object o : elements) {
+			// Objects are rows
+			Array a = (Array)o;
+			result.addElement(a.inner(other));
+		}
+		return result;
+	}
+
+	/*
+	 * Other array is matrix (this is matrix)
+	 */
+	public Array matrixMulMatrix(Array other) {
+		Array result = new Array();
+		Array transposed = other.transposed();
+		for(Object rightColumn_ : transposed.elements()) {
+			Array rightColumn = (Array)rightColumn_;
+			Array resultColumn = new Array();
+			for(Object leftRow_ : elements) {
+				Array leftRow = (Array)leftRow_;
+				resultColumn.addElement(leftRow.inner(rightColumn));
+			}
+			result.addElement(resultColumn);
+		}
+		return result.transposed();
+	}
+
 	public Array pow(Array other) {
 		Array result = new Array();
 		Collection<Object> lae = elements();
@@ -535,5 +569,30 @@ public class Array implements IExpression {
 	public void applyPartial(Array[] indices, Array value) {
 		applyPartial(indices, value, 0);
 	}
+	
+	public Array transposed() {
+		
+		List<Object> elements = elements();
+		if(elements.isEmpty()) return this;
+		
+		Array first = (Array)elements.get(0);
+		
+		Array[] arrs = new Array[first.elements().size()];
+		for(int i=0;i<arrs.length;i++) arrs[i] = new Array();
+		
+		for(int i=0;i<elements.size();i++) {
+			Array a = (Array)elements.get(i);
+			for(int j=0;j<a.elements().size();j++) {
+				Object o = a.element(j);
+				arrs[j].addElement(o);
+			}
+		}
+
+		Array result = new Array();
+		for(Array a : arrs) result.addElement(a);
+
+		return result;
+		
+	}
 
 }
diff --git a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Environment.java b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Environment.java
index 34f6319b..135410d7 100644
--- a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Environment.java
+++ b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Environment.java
@@ -397,7 +397,7 @@ final public class Environment implements IEnvironment, ISystem {
 			public Object evaluate(IEnvironment environment, int argc) {
 				int[] dims = new int[argc];
 				for(int i=0;i<argc;i++) {
-					Double d = (Double)environment.getValue(0);
+					Double d = (Double)environment.getValue(i);
 					dims[i] = d.intValue();
 				}
 				return make(dims, 0);
@@ -427,6 +427,17 @@ final public class Environment implements IEnvironment, ISystem {
 				return make(dims, 0);
 			}
 			
+		});
+		model.functions.put("transpose", new Fn1(1) {
+
+			@Override
+			public Object evaluate(IEnvironment environment, int argc) {
+				
+				Array array = (Array)environment.getValue(0);
+				return array.transposed();
+				
+			}
+			
 		});
 		model.functions.put("delay", new Fn1(4) {
 			
diff --git a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Multiplication.java b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Multiplication.java
index 154d4e91..97791824 100644
--- a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Multiplication.java
+++ b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Multiplication.java
@@ -40,6 +40,10 @@ public class Multiplication implements IExpression {
 			Array ra = (Array)right;
 			if(la.isVector && ra.isVector) {
 				return la.inner(ra);
+			} else if(ra.isVector) {
+				return la.matrixMulVector(ra);
+			} else {
+				return la.matrixMulMatrix(ra);
 			}
 		}
 		else if (left instanceof Array && right instanceof Double) return ((Array)left).mul((Double)right);
diff --git a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Solver.java b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Solver.java
index 4d083f77..d66eb549 100644
--- a/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Solver.java
+++ b/fi.semantum.sysdyn.solver/src/fi/semantum/sysdyn/solver/Solver.java
@@ -28,7 +28,7 @@ import fi.semantum.sysdyn.solver.parser.SimpleNode;
 
 public class Solver {
 
-	private static final boolean VALIDATE = false;	
+	private static final boolean VALIDATE = true;	
 	private static final boolean PRINT_EXCEPTIONS = false;	
 	private static final int STACK_SIZE = 1000;
 	private static final int MAX_LOOPS = 50;
@@ -115,8 +115,13 @@ public class Solver {
 		if(VALIDATE) {
 			if(value instanceof Double) {
 				if(Double.isNaN((Double)value)) {
+					System.err.println("value is invalid (NaN): " + var.base.name);
 					throw new IllegalStateException("value is invalid (NaN)");
 				}
+				if(Double.isInfinite((Double)value)) {
+					System.err.println("value is invalid (Infinite): " + var.base.name);
+					throw new IllegalStateException("value is invalid (Infinite)");
+				}
 			}
 			if(value instanceof Array) {
 				for(Object o : ((Array)value).elements()) validate(var, o);
-- 
2.47.1