missing function

beast-dev · Sep 25, 2023 · eb2a370 · eb2a370
1 parent 036828f
commit eb2a370
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/src/dr/evomodel/substmodel/DifferentiableSubstitutionModelUtil.java b/src/dr/evomodel/substmodel/DifferentiableSubstitutionModelUtil.java
@@ -328,6 +328,25 @@ public static double[] getQQPlus(double[] eigenVectors, double[] inverseEigenVec
         return result;
     }
 
+    public static double[] getQPlusQ(double[] eigenVectors, double[] inverseEigenVectors, int index, int stateCount) {
+
+        double[] result = new double[stateCount * stateCount];
+
+        for (int i = 0; i < stateCount; ++i) {
+            for (int j = 0; j < stateCount; ++j) {
+                double sum = 0.0;
+                for (int k = 0; k < stateCount; ++k) {
+                    if (k != index) {
+                        sum += inverseEigenVectors[i * stateCount + k] * eigenVectors[k * stateCount + j];
+                    }
+                }
+                result[i * stateCount + j] = sum;
+            }
+        }
+
+        return result;
+    }
+
     private static int index12(int i, int j, int stateCount) {
         return i * stateCount + j;
     }