From 05a2933b998a1bd6aefe6104ba72ca6dc35dbd07 Mon Sep 17 00:00:00 2001
From: Tilmann <zoodb@gmx.de>
Date: Sun, 28 Apr 2024 14:07:05 +0200
Subject: [PATCH] Fix #133

---
 CHANGELOG.md                                  |  5 +-
 .../java/org/ode4j/ode/internal/DLCP.java     | 71 +++++++-------
 .../ode/TestIssue0133_LcpInternalError.java   | 96 +++++++++++++++++++
 3 files changed, 137 insertions(+), 35 deletions(-)
 create mode 100644 core/src/test/java/org/ode4j/ode/TestIssue0133_LcpInternalError.java

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9178b67c..044f7722 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -24,12 +24,15 @@
     WHen using JDK 9 or later
 
 --> See TODO.txt
+## 0.5.3 - unreleased
+- Fix INTERNAL ERROR in `FastLSolve.solveL1Straight()` caused by bug in `DLCP.solve1()`.
+  [#133](https://github.com/tzaeschke/ode4j/issues/133)
 
 ## 0.5.2 - 2023-10-07
 - Fix DVector3.cross() returning always 0. [#128](https://github.com/tzaeschke/ode4j/issues/128)
 - Fix JUnit test lint. [#130](https://github.com/tzaeschke/ode4j/pull/130)
 - Fix Demo window size to minimum 640x480. [#131](https://github.com/tzaeschke/ode4j/pull/131)
-- Add javadoc to dindicate that angular velocity is given in radians. [#132](https://github.com/tzaeschke/ode4j/pull/132)
+- Add javadoc to indicate that angular velocity is given in radians. [#132](https://github.com/tzaeschke/ode4j/pull/132)
 
 ## 0.5.1 - 2023-09-17
 - Support for HiDPI screens / Apple Silicon/Retina. Contribution by valb3r,
diff --git a/core/src/main/java/org/ode4j/ode/internal/DLCP.java b/core/src/main/java/org/ode4j/ode/internal/DLCP.java
index 670dbc09..9a85835f 100644
--- a/core/src/main/java/org/ode4j/ode/internal/DLCP.java
+++ b/core/src/main/java/org/ode4j/ode/internal/DLCP.java
@@ -916,41 +916,44 @@ void solve1 (double[] a, int i, boolean dir_positive, boolean only_transfer)
 
 		final int nC = m_nC;
 		if (nC > 0) {
-		    double[] Dell = m_Dell;
-		    int[] C = m_C;
-			//double[] aptr = AROW(i);
-			int aPos = AROWp(i);
-			if (NUB_OPTIMIZATIONS) {//#   ifdef NUB_OPTIMIZATIONS
-				// if nub>0, initial part of aptr[] is guaranteed unpermuted
-			    final int nub = m_nub;
-			    int j=0;
-				for ( ; j<nub; ++j) Dell[j] = m_A[aPos+j];//aptr[j];
-				for ( ; j<nC; ++j) Dell[j] = m_A[aPos+C[j]];//aptr[C[j]];
-			} else {//#   else
-        		for (int j=0; j<nC; j++) Dell[j] = m_A[aPos+C[j]];//aptr[C[j]];
-			}//#   endif
-		}
-		solveL1Straight(m_L, m_Dell, 0, nC,  m_nskip, 1);
-		{
-		    double[] ell = m_ell, Dell = m_Dell, d = m_d;
-		    for (int j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
-		}
+			{
+				double[] Dell = m_Dell;
+				int[] C = m_C;
+				//double[] aptr = AROW(i);
+				int aPos = AROWp(i);
+				if (NUB_OPTIMIZATIONS) {//#   ifdef NUB_OPTIMIZATIONS
+					// if nub>0, initial part of aptr[] is guaranteed unpermuted
+					final int nub = m_nub;
+					int j = 0;
+					for (; j < nub; ++j) Dell[j] = m_A[aPos + j];//aptr[j];
+					for (; j < nC; ++j) Dell[j] = m_A[aPos + C[j]];//aptr[C[j]];
+				} else {//#   else
+					for (int j = 0; j < nC; j++) Dell[j] = m_A[aPos + C[j]];//aptr[C[j]];
+				}//#   endif
+			}
 
-		if (!only_transfer) {
-		    double[] tmp = m_tmp, ell = m_ell;
-		    {
-		        for (int j=0; j<nC; ++j) tmp[j] = ell[j];
-		    }
-			solveL1Transposed(m_L,tmp,0, nC,m_nskip, 1);
-		    if (dir_positive) {
-		        int[] C = m_C;
-		        //double[] tmp = m_tmp;
-		        for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j];
-		    } else {
-		        int[] C = m_C;
-		        //double[] tmp = m_tmp;
-		        for (int j=0; j<nC; ++j) a[C[j]] = tmp[j];
-		    }
+			solveL1Straight(m_L, m_Dell, 0, nC, m_nskip, 1);
+			{
+				double[] ell = m_ell, Dell = m_Dell, d = m_d;
+				for (int j = 0; j < nC; j++) ell[j] = Dell[j] * d[j];
+			}
+
+			if (!only_transfer) {
+				double[] tmp = m_tmp, ell = m_ell;
+				{
+					for (int j = 0; j < nC; ++j) tmp[j] = ell[j];
+				}
+				solveL1Transposed(m_L, tmp, 0, nC, m_nskip, 1);
+				if (dir_positive) {
+					int[] C = m_C;
+					//double[] tmp = m_tmp;
+					for (int j = 0; j < nC; ++j) a[C[j]] = -tmp[j];
+				} else {
+					int[] C = m_C;
+					//double[] tmp = m_tmp;
+					for (int j = 0; j < nC; ++j) a[C[j]] = tmp[j];
+				}
+			}
 		}
 	}
 
diff --git a/core/src/test/java/org/ode4j/ode/TestIssue0133_LcpInternalError.java b/core/src/test/java/org/ode4j/ode/TestIssue0133_LcpInternalError.java
new file mode 100644
index 00000000..8524107a
--- /dev/null
+++ b/core/src/test/java/org/ode4j/ode/TestIssue0133_LcpInternalError.java
@@ -0,0 +1,96 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine 4J, Copyright (C) 2009-2014 Tilmann Zaeschke     *
+ * All rights reserved.  Email: ode4j@gmx.de   Web: www.ode4j.org        *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file ODE-LICENSE-BSD.TXT and ODE4J-LICENSE-BSD.TXT.         *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT, ODE-LICENSE-BSD.TXT and ODE4J-LICENSE-BSD.TXT for more   *
+ * details.                                                              *
+ *                                                                       *
+ *************************************************************************/
+package org.ode4j.ode;
+
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
+
+/**
+ * Issue #133: ODE INTERNAL ERROR in FastLSolve.solveL1Straight().
+ */
+public class TestIssue0133_LcpInternalError {
+
+    private final DContactBuffer contacts = new DContactBuffer(4);
+    private DWorld world;
+    private DJointGroup contactGroup;
+
+    @Before
+    public void beforeTest() {
+        OdeHelper.initODE2(0);
+    }
+
+    @After
+    public void afterTest() {
+        OdeHelper.closeODE();
+    }
+
+    private void nearCallback(Object data, DGeom o1, DGeom o2) {
+        DBody b1 = o1.getBody();
+        DBody b2 = o2.getBody();
+        if (b1 != null && b2 != null && OdeHelper.areConnected(b1, b2)) return;
+
+        int n = OdeHelper.collide(o1, o2, OdeConstants.dContactBounce, contacts.getGeomBuffer());
+        if (n > 0) {
+            for (int i = 0; i < n; i++) {
+                DContact contact = contacts.get(i);
+                DJoint contactJoint = OdeHelper.createContactJoint(world, contactGroup, contact);
+                contactJoint.attach(o1.getBody(), o2.getBody());
+            }
+        }
+    }
+
+    /**
+     * This crashed due to a bug in DLCP.solve1() with: <br>
+     * java.lang.RuntimeException: #1: assertion failed
+     * at org.ode4j/org.ode4j.ode.internal.ErrorHdl.dDebug(ErrorHdl.java:146)
+     * at org.ode4j/org.ode4j.ode.internal.ErrorHandler.dDebug(ErrorHandler.java:101)
+     * at org.ode4j/org.ode4j.ode.internal.Common.dIASSERT(Common.java:128)
+     * at org.ode4j/org.ode4j.ode.internal.FastLSolve.solveL1Straight(FastLSolve.java:49)
+     * at org.ode4j/org.ode4j.ode.internal.DLCP.solve1(DLCP.java:935)
+     * at org.ode4j/org.ode4j.ode.internal.DLCP.solve1(DLCP.java:545)
+     */
+    @Test
+    public void test() {
+        double ballRadius = 5.0;
+        double ballMass = 23.0;
+        contactGroup = OdeHelper.createJointGroup();
+        world = OdeHelper.createWorld();
+        world.setGravity(0, 0, -9.81);
+        DSpace space = OdeHelper.createSimpleSpace();
+        DBody ballBody = OdeHelper.createBody(world);
+        DGeom ballGeom = OdeHelper.createSphere(space, ballRadius);
+        ballGeom.setBody(ballBody);
+        DMass m = OdeHelper.createMass();
+        m.setSphereTotal(ballMass, ballRadius);
+        ballBody.setMass(m);
+        ballBody.setPosition(0, 0, ballRadius * 3);
+        OdeHelper.createPlane(space, 0, 0, 1, 0);
+
+        for (int i = 0; i < 10000; i++) {
+            OdeHelper.spaceCollide(space, null, (data, o1, o2) -> nearCallback(data, o1, o2));
+            world.step(0.01);
+            contactGroup.empty();
+        }
+    }
+}