Index: build/premake4.lua
===================================================================
--- build/premake4.lua	(revision 1903)
+++ build/premake4.lua	(working copy)
@@ -32,6 +32,8 @@
     "kinematic",
     "motion",
     "motor",
+    "amplus",
+    "lmplus",
     "ode",
     "piston",
     "plane2d",
Index: include/ode/amplus_api.h
===================================================================
--- include/ode/amplus_api.h	(revision 0)
+++ include/ode/amplus_api.h	(working copy)
@@ -0,0 +1,242 @@
+/*************************************************************************
+ *                                                                       *
+ * AMPlus for Open Dynamics Engine.  Copyright (C) 2012 Joseph Cooper.   *
+ *                                                                       *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *   (3) The zlib/libpng license that is included with this library in   *
+ *       the file LICENSE-ZLIB.TXT                                       *
+ *                                                                       *
+ * This library is distributed WITHOUT ANY WARRANTY, including implied   *
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
+ * See the files LICENSE.TXT or LICENSE-BSD.TXT or LICENSE-ZLIB.TXT      *
+ *                                                                       *
+ *************************************************************************/
+
+
+/**
+ * @brief Create a new joint of the AMPlus type.
+ * @ingroup joints
+ * @param dJointGroupID set to 0 to allocate the joint normally.
+ * If it is nonzero the joint is allocated in the given joint group.
+ */
+ODE_API dJointID dJointCreateAMPlus (dWorldID, dJointGroupID);
+
+/**
+ * @brief set joint parameter
+ * @remarks This is how the degree of freedom is controlled
+ * if the histop==lostop then the constraint is a hard constraint
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusParam( dJointID j, int parameter, dReal value );
+
+/**
+ * @brief get joint parameter
+ * @ingroup joints
+ */
+ODE_API dReal dJointGetAMPlusParam( dJointID j, int parameter );
+
+/**
+ * @brief Set an AMPlus anchor axis for the first body.
+ * @param anum selects the axis to change (0,1 or 2).
+ * @remarks The axis vector is specified in global coordinates but
+ * will be stored relative to the first body
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusAxis1( dJointID j, int anum, dReal x, dReal y, dReal z);
+
+/**
+ * @brief Set an AMPlus anchor axis for the first body.
+ * @param anum selects the axis to change (0,1 or 2).
+ * @remarks The axis vector is specified relative to the first body
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusAxis1Rel( dJointID j, int anum, dReal x, dReal y, dReal z);
+ODE_API void dJointSetAMPlusAxis1V( dJointID j, int anum, const dVector3 ax);
+ODE_API void dJointSetAMPlusAxis1RelV( dJointID j, int anum, const dVector3 ax);
+
+/**
+ * @brief Set an AMPlus anchor axis for the second body.
+ * @param anum selects the axis to change (0,1 or 2).
+ * @remarks The axis vector is specified in global coordinates but
+ * will be stored relative to the second body
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusAxis2( dJointID j, int anum, dReal x, dReal y, dReal z);
+
+/**
+ * @brief Set an AMPlus anchor axis for the second body.
+ * @param anum selects the axis to change (0,1 or 2).
+ * @remarks The axis vector is specified relative to the second body
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusAxis2Rel( dJointID j, int anum, dReal x, dReal y, dReal z);
+ODE_API void dJointSetAMPlusAxis2V( dJointID j, int anum, const dVector3 ax);
+ODE_API void dJointSetAMPlusAxis2RelV( dJointID j, int anum, const dVector3 ax);
+
+/**
+ * @brief Set the AMPlus reference axis.
+ * @param anum selects the axis to change (0, 1 or 2).
+ * @param frame Each axis can have one of two ``relative orientations'' 
+ * \li 1: The axis is anchored to the first body.
+ * \li 2: The axis is anchored to the second body.
+ * @remarks This defines the axis of rotation in projected mode
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusAxisRef( dJointID j, int anum, int frame,  dReal x, dReal y, dReal z);
+ODE_API void dJointSetAMPlusAxisRefRel( dJointID j, int anum, int frame,  dReal x, dReal y, dReal z);
+ODE_API void dJointSetAMPlusAxisRefV( dJointID j, int anum, int frame, const dVector3 ax);
+ODE_API void dJointSetAMPlusAxisRefRelV( dJointID j, int anum, int frame, const dVector3 ax);
+
+/**
+ * @brief set AMPlus mode for the specified axis
+ * @param anum selects the axis to change (0,1 or 2).
+ * @ingroup joints
+ * @param mode Each axis can have one of two angle modes
+ * \li dAMPlusCone: The joint constrains the absolute angle between 
+ * the two anchor axes.
+ * \li dAMPlusProjected: The joint constrains the angle between 
+ * the projection of the two anchor axes onto the planespace 
+ * orthogonal to the reference axis.  The reference axis is the
+ * the axis of rotation.
+ */
+ODE_API void dJointSetAMPlusMode( dJointID j, int anum, int mode);
+
+/**
+ * @brief This convenience function establishes the angular 
+ * portion of a hinge constraint between two bodies 
+ * @remarks
+ * Axis 0 and Axis 1 become constraints to keep the hinge 
+ * axis aligned relative to the two bodies
+ * Axis 2 becomes a projection constraint that controls the
+ * hinge angle
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusHinge( dJointID j, dReal x, dReal y, dReal z);
+ODE_API void dJointSetAMPlusHingeV( dJointID j, const dVector3 ax);
+
+/**
+ * @brief This convenience function establishes the angular 
+ * portion of a universal constraint between two bodies 
+ * @remarks
+ * Axis 0 becomes a constraint to keep the crosspiece axes orthogonal
+ * Axes 1 and 2 control rotation around the two degrees of freedom
+ * @ingroup joints
+ */
+ODE_API void dJointSetAMPlusUniversal( dJointID j, dReal x1, dReal y1, dReal z1,
+																			dReal x2, dReal y2, dReal z2);
+ODE_API void dJointSetAMPlusUniversalV( dJointID j, const dVector3 ax1, const dVector3 ax2);
+
+/**
+ * @brief get anchor axis for body 1
+ * @param anum selects the axis (0,1 or 2).
+ * @remarks The axis is transformed to world coordinates 
+ * and stored in @param res
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusAxis1( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief get anchor axis for body 1
+ * @param anum selects the axis (0,1 or 2).
+ * @remarks The axis is kept in body1 coordinates 
+ * and copied to @param res
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusAxis1Rel( const dJointID j, int anum, dVector3 res);
+/**
+ * @brief get anchor axis for body 2
+ * @param anum selects the axis (0,1 or 2).
+ * @remarks The axis is transformed to world coordinates 
+ * and stored in @param res
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusAxis2( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief get anchor axis for body 2
+ * @param anum selects the axis (0,1 or 2).
+ * @remarks The axis is kept in body2 coordinates 
+ * and copied to @param res
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusAxis2Rel( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief get the reference axis
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @remarks The axis is returned in global coordinates
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusAxisRef( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief get the reference axis
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @remarks The axis is returned in whatever frames it set to
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusAxisRefRel( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief get the frame for the reference axis
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @ingroup joints
+ */
+ODE_API int dJointGetAMPlusAxisRefFrame( const dJointID j, int anum);
+
+/**
+ * @brief get the mode for the specified degree of freedom
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @ingroup joints
+ */
+ODE_API int dJointGetAMPlusMode( const dJointID j, int anum);
+
+/**
+ * @brief Find the effective axis of rotation for the specified
+ * degree of freedom
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @remarks In Cone mode, the effective axis is the 
+ * cross-product of the two axes.  In Projected mode,
+ * it's the reference axis.
+ * @ingroup joints
+ */
+ODE_API void dJointGetAMPlusEffectiveAxis( const dJointID j, int anum, dVector3 res );
+
+/**
+ * @brief get angle between the anchor axes
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @remarks In cone mode, the angle is the arccos of
+ * the dot-product of the two anchor axes.  In projected
+ * mode, the angle is how far one anchor axis is rotated
+ * around from reference axis from the other anchor axis. 
+ * @ingroup joints
+ */
+ODE_API dReal dJointGetAMPlusAngle( const dJointID j, int anum );
+
+/**
+ * @brief Get the angle time derivative.
+ * @ingroup joints
+ */
+ODE_API dReal dJointGetAMPlusAngleRate( const dJointID j, int anum );
+
+/**
+ * @brief Applies the torque about the effective axis.
+ * @param anum selects the degree of freedom (0,1 or 2).
+ * @remarks 
+ * It applies a torque with specified magnitude in the direction
+ * of the effective axis, to body 1, and with the same magnitude but in opposite
+ * direction to body 2. This function is just a wrapper for dBodyAddTorque()}
+ * @ingroup joints
+ */
+ODE_API void dJointAddAMPlusTorque(dJointID j,int anum, dReal torque);
+
+
Index: include/ode/common.h
===================================================================
--- include/ode/common.h	(revision 1903)
+++ include/ode/common.h	(working copy)
@@ -110,6 +110,7 @@
 #define dCos(x) (cosf(x))				/* cosine */
 #define dFabs(x) (fabsf(x))			/* absolute value */
 #define dAtan2(y,x) (atan2f(y,x))		/* arc tangent with 2 args */
+#define dAcos(x) (acosf(x))
 #define dFMod(a,b) (fmodf(a,b))		/* modulo */
 #define dFloor(x) floorf(x)			/* floor */
 #define dCeil(x) ceilf(x)			/* ceil */
@@ -145,6 +146,7 @@
 #define dCos(x) cos(x)
 #define dFabs(x) fabs(x)
 #define dAtan2(y,x) atan2((y),(x))
+#define dAcos(x) acos(x)
 #define dFMod(a,b) (fmod((a),(b)))
 #define dFloor(x) floor(x)
 #define dCeil(x) ceil(x)
@@ -213,7 +215,9 @@
   dJointTypePU,
   dJointTypePiston,
   dJointTypeDBall,
-  dJointTypeDHinge
+  dJointTypeDHinge,
+  dJointTypeLMPlus,
+  dJointTypeAMPlus,
 } dJointType;
 
 
@@ -255,6 +259,8 @@
   dParamLoStop = start, \
   dParamHiStop, \
   dParamVel, \
+  dParamLoVel, \
+  dParamHiVel, \
   dParamFMax, \
   dParamFudgeFactor, \
   dParamBounce, \
@@ -280,6 +286,8 @@
   dParamLoStop ## x = start, \
   dParamHiStop ## x, \
   dParamVel ## x, \
+  dParamLoVel ## x, \
+  dParamHiVel ## x, \
   dParamFMax ## x, \
   dParamFudgeFactor ## x, \
   dParamBounce ## x, \
@@ -312,7 +320,19 @@
   dAMotorEuler = 1
 };
 
+/* LMPlus mode numbers */
+enum {
+	dLMPlusPlanar = 0,
+	dLMPlusCylindrical,
+	dLMPlusSpherical
+};
 
+/* AMPlus mode numbers */
+enum {
+  dAMPlusCone = 0,
+  dAMPlusProjected
+};
+
 /* joint force feedback information */
 
 typedef struct dJointFeedback {
Index: include/ode/lmplus_api.h
===================================================================
--- include/ode/lmplus_api.h	(revision 0)
+++ include/ode/lmplus_api.h	(working copy)
@@ -0,0 +1,192 @@
+/*************************************************************************
+ *                                                                       *
+ * LMPlus for Open Dynamics Engine.  Copyright (C) 2012 Joseph Cooper.   *
+ *                                                                       *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *   (3) The zlib/libpng license that is included with this library in   *
+ *       the file LICENSE-ZLIB.TXT                                       *
+ *                                                                       *
+ * This library is distributed WITHOUT ANY WARRANTY, including implied   *
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
+ * See the files LICENSE.TXT or LICENSE-BSD.TXT or LICENSE-ZLIB.TXT      *
+ *                                                                       *
+ *************************************************************************/
+
+
+/**
+ * @brief Create a new joint of the LMPlus type.
+ * @ingroup joints
+ * @param dJointGroupID set to 0 to allocate the joint normally.
+ * If it is nonzero the joint is allocated in the given joint group.
+ */
+ODE_API dJointID dJointCreateLMPlus (dWorldID, dJointGroupID);
+
+/**
+ * @brief set joint parameter
+ * @remarks This is how the degree of freedom is controlled
+ * if the histop==lostop then the constraint is a hard constraint
+ * @ingroup joints
+ */
+ODE_API void dJointSetLMPlusParam( dJointID j, int parameter, dReal value );
+
+/**
+ * @brief get joint parameter
+ * @ingroup joints
+ */
+ODE_API dReal dJointGetLMPlusParam( dJointID j, int parameter );
+
+/**
+ * @brief set the anchors for both bodies
+ * @remarks the anchor points are expected to be in global coordinates
+ * the distance between the anchors will be constrained along
+ * the specified axis.
+ * @ingroup joints
+ */
+ODE_API void dJointSetLMPlusAnchors( dJointID j, dReal x1, dReal y1, dReal z1, dReal x2, dReal y2, dReal z2);
+ODE_API void dJointSetLMPlusAnchorsV( dJointID j, const dVector3 a1, const dVector3 a2);
+ODE_API void dJointSetLMPlusAnchorsRel( dJointID j, dReal x1, dReal y1, dReal z1, dReal x2, dReal y2, dReal z2);
+ODE_API void dJointSetLMPlusAnchorsRelV( dJointID j, const dVector3 a1, const dVector3 a2);
+
+/**
+ * @brief set the anchors for the first body
+ * @remarks the anchor point is expected to be in global coordinates
+ * @ingroup joints
+ */
+ODE_API void dJointSetLMPlusAnchor1( dJointID j, dReal x1, dReal y1, dReal z1);
+ODE_API void dJointSetLMPlusAnchor1V( dJointID j, const dVector3 a1);
+ODE_API void dJointSetLMPlusAnchor1Rel( dJointID j, dReal x1, dReal y1, dReal z1);
+ODE_API void dJointSetLMPlusAnchor1RelV( dJointID j, const dVector3 a1);
+
+/**
+ * @brief set the anchors for the second body
+ * @remarks the anchor point is expected to be in global coordinates
+ * @ingroup joints
+ */
+ODE_API void dJointSetLMPlusAnchor2( dJointID j, dReal x2, dReal y2, dReal z2);
+ODE_API void dJointSetLMPlusAnchor2V( dJointID j, const dVector3 a2);
+ODE_API void dJointSetLMPlusAnchor2Rel( dJointID j, dReal x2, dReal y2, dReal z2);
+ODE_API void dJointSetLMPlusAnchor2RelV( dJointID j, const dVector3 a2);
+
+/**
+ * @brief Set the LMPlus axis.
+ * @param anum selects the axis to change (0, 1 or 2).
+ * @param frame Each axis can be fixed relative to one of three reference frames
+ * \li 0: The axis is anchored to the world.
+ * \li 1: The axis is anchored to the first body.
+ * \li 2: The axis is anchored to the second body.
+ * @remarks This defines the axis of constraint for the two anchor points
+ * This function is unnecessary for spherical mode
+ * @ingroup joints
+ */
+ODE_API void dJointSetLMPlusAxis( dJointID j, int anum, int frame,  dReal x, dReal y, dReal z);
+ODE_API void dJointSetLMPlusAxisV( dJointID j, int anum, int frame, const dVector3 ax);
+ODE_API void dJointSetLMPlusAxisRel( dJointID j, int anum, int frame,  dReal x, dReal y, dReal z);
+ODE_API void dJointSetLMPlusAxisRelV( dJointID j, int anum, int frame, const dVector3 ax);
+
+/**
+ * @brief set AMPlus mode for the specified axis
+ * @param anum selects the axis to change (0,1 or 2).
+ * 
+ * @param mode Each axis can have one of three modes
+ * \li dLMPlusPlanar: The directed distance between the anchor points
+ * along the specifed axis is constrained.  If the distance is set
+ * to zero, a single constraint axis keeps the anchors in
+ * the same plane.
+ * \li dLMPlusCylindrical: The absolute distance between the anchor
+ * points projected onto a plane orthogonal to the specified axis is
+ * constrained.  If the distance is set to a constant, then one anchor
+ * point is kept on the surface of an infinite cylinder relative to the other.
+ * \li dLMPlusSpherical: The absolute distance between the anchor
+ * points is constrained.  The axis specified for this @param anum has
+ * no effect.  If the distance is set to a constant, then one anchor
+ * point is kept on the surface of a sphere relative to the other.
+ * @ingroup joints
+ */
+ODE_API void dJointSetLMPlusMode( dJointID j, int anum, int mode);
+
+/**
+ * @brief Get the LMPlus anchor point for body1, in world coordinates.
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusAnchor1( const dJointID j, dVector3 res);
+
+/**
+ * @brief Get the LMPlus anchor point for body1, in body1 coordinates.
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusAnchor1Rel( const dJointID j, dVector3 res);
+
+/**
+ * @brief Get the LMPlus anchor point for body2, in world coordinates.
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusAnchor2( const dJointID j, dVector3 res);
+
+/**
+ * @brief Get the LMPlus anchor point for body2, in body2 coordinates.
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusAnchor2Rel( const dJointID j, dVector3 res);
+
+/**
+ * @brief Get the LMPlus axis for degree of freedom specified by @param anum (0,1,2)
+ * In world coordinates.
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusAxis( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief Get the LMPlus axis for degree of freedom specified by @param anum (0,1,2)
+ * In the DoF's specified FoR's coordinates.
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusAxisRel( const dJointID j, int anum, dVector3 res);
+
+/**
+ * @brief Get the frame of reference designated for the degree of freedom
+ * specified by @param anum
+ * @ingroup joints
+ */
+ODE_API int dJointGetLMPlusAxisFrame( const dJointID j, int anum);
+
+/**
+ * @brief Get the mode (planar, cylindrical, spherical) designated 
+ * for the degree of freedom specified by @param anum (0,1,2)
+ * @ingroup joints
+ */
+ODE_API int dJointGetLMPlusMode( const dJointID j, int anum);
+
+/**
+ * @brief Get the effective axis of constraint
+ * In Planar mode, this is the same as GetLMPlusAxis.
+ * In Spherical mode, this will be the normalized vector between the
+ * two anchor points.
+ * In Cylindrical mode, this be the projection of the vector
+ * between the two anchor points onto the plane defined by
+ * by the axis
+ * @ingroup joints
+ */
+ODE_API void dJointGetLMPlusEffectiveAxis( const dJointID j, int anum, dVector3 res );
+
+/**
+ * @brief The distance between the anchor points along the effective axis.
+ * @ingroup joints
+ */
+ODE_API dReal dJointGetLMPlusSeparation( const dJointID j, int anum );
+
+/**
+ * @brief Time derivative of teh distance between the anchor points along the 
+ * effective axis.
+ * @ingroup joints
+ */
+ODE_API dReal dJointGetLMPlusSeparationRate( const dJointID j, int anum );
+
Index: include/ode/Makefile.am
===================================================================
--- include/ode/Makefile.am	(revision 1903)
+++ include/ode/Makefile.am	(working copy)
@@ -15,6 +15,8 @@
 				collision.h \
 				error.h \
 				objects.h \
+                        amplus_api.h \
+                        lmplus_api.h \
 				rotation.h \
 				collision_space.h \
 				export-dif.h \
Index: include/ode/objects.h
===================================================================
--- include/ode/objects.h	(revision 1903)
+++ include/ode/objects.h	(working copy)
@@ -3105,6 +3105,9 @@
  */
 ODE_API dReal dJointGetDHingeParam(dJointID, int parameter);
 
+/* The objects.h file is way too big. */
+#include <ode/lmplus_api.h>
+#include <ode/amplus_api.h>
 
 
 
Index: include/ode/odemath.h
===================================================================
--- include/ode/odemath.h	(revision 1903)
+++ include/ode/odemath.h	(working copy)
@@ -423,6 +423,17 @@
  */
 
 ODE_API void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
+
+/*
+ * Given unit normal vector 'n', find normalized projection 'p' of vector
+ * 'd' onto the planespace and orthonormal vector 'q'.
+ * If 'n' is parallel to 'd', then this will be the same as dPlaneSpace
+ * and return value will be 0.
+ * Otherwise, return value is 1.
+ */
+ODE_API int dDirectedPlaneSpace  (const dVector3 n, const dVector3 d, dVector3 p, dVector3 q);
+
+
 /* Makes sure the matrix is a proper rotation */
 ODE_API void dOrthogonalizeR(dMatrix3 m);
 
Index: ode/demo/demo_amplus.cpp
===================================================================
--- ode/demo/demo_amplus.cpp	(revision 0)
+++ ode/demo/demo_amplus.cpp	(working copy)
@@ -0,0 +1,497 @@
+/*************************************************************************
+ * AMPlus demo, written as part of the                                   *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ *   This demo was built from demo_chain1.c                              *
+ *   Content novel to this file Copyright 2012, Joseph Cooper            *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * Novel material in this file can also, at your option, be distributed  *
+ * under the ZLib license,                                               *
+ * see http://www.gzip.org/zlib/zlib_license.html                        *
+ *                                                                       *
+ * 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 and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+/* exercise the C interface */
+
+#include <stdio.h>
+#include "ode/ode.h"
+#include "drawstuff/drawstuff.h"
+#include "texturepath.h"
+
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+
+/* select correct drawing functions */
+
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCapsule dsDrawCapsuleD
+#define dsDrawLine dsDrawLineD
+#endif
+
+
+
+
+#define DENSITY (1000.0f) 
+
+/* dynamics and collision objects */
+static dWorldID world;
+static dSpaceID space;
+static dJointGroupID contactgroup;
+
+/* A 'plane' */
+static dBodyID planeBody;
+static dJointID planeAnchor;
+static dJointID planeLimit;
+static dReal planeDim[3] = {4,4,.1};
+
+/* A bunch of balls */
+#define SPHERENUM 1
+static dBodyID sphereBody[SPHERENUM];
+static dJointID sphereJoint[SPHERENUM];
+static dReal sphereRad = 0.1;
+
+/* Flippers */
+#define FLIPNUM 4
+static dBodyID lFlipper[FLIPNUM];
+static dJointID lFlipHinge[FLIPNUM];
+static dBodyID rFlipper[FLIPNUM];
+static dJointID rFlipHinge[FLIPNUM];
+static dReal flipDim[3] = {1.5,.1,.2};
+
+/* Tethered spinners*/
+#define SPINNUM 4
+static dBodyID spinner[SPINNUM];
+static dJointID spinAMP[SPINNUM];
+static dJointID spinLMP[SPINNUM];
+static dReal spinDim[3] = {.4,.05,.2};
+
+#define PADNUM 4
+static dBodyID paddle[PADNUM];
+static dJointID padAMP[PADNUM];
+static dJointID padLMP[PADNUM];
+static dReal padDim[3] = {.4,.05,.2};
+
+
+
+/* Popper */
+//static dBodyID popper;
+//static dJointID popAMP;
+//static dJointID popLMP;
+//static dReal popDim[3] = {0.5,0.5,0.01};
+
+static int lCntDown = 0;
+static int rCntDown = 0;
+/* this is called by dSpaceCollide when two objects in space are
+ * potentially colliding.
+ */
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+{
+  /* exit without doing anything if the two bodies are connected by a joint */
+  dBodyID b1,b2;
+  dContact contact;
+
+  b1 = dGeomGetBody(o1);
+  b2 = dGeomGetBody(o2);
+
+  if (b1 && b2 && dAreConnectedExcluding( b1,b2,dJointTypeLMPlus)) return;
+  //if (b1 && b2 && dAreConnected( b1,b2)) return;
+
+  contact.surface.mode = 0;
+  contact.surface.mu = 0.1;
+  contact.surface.mu2 = 0;
+  if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
+    dJointID c = dJointCreateContact (world,contactgroup,&contact);
+    dJointAttach (c,b1,b2);
+  }
+}
+
+
+/* start simulation - set viewpoint */
+
+static void start()
+{
+  static float xyz[3] = {2.1640f,-5.3079f,2.7600f};
+  static float hpr[3] = {110.0000f,0.0000f,0.0000f};
+
+  dAllocateODEDataForThread(dAllocateMaskAll);
+  dsSetViewpoint (xyz,hpr);
+
+  printf("'z'   - left paddles\n");
+  printf("'/'   - right paddles\n");
+  printf("'s'   - stop all\n");
+  printf("space - tilt\n");
+}
+
+void stopall();
+/* simulation loop */
+static void simLoop (int pause)
+{
+  int i;
+  static dReal tv = 0;
+  static int reset = 20;
+
+  if (!pause) {
+    /* Because we're lazy and create all the joints
+       in an unsatisfied state, we kill all of the
+       momentum after a fixed number of frames (once
+       most of the error is resolved). */
+    if (reset>0) {
+      --reset;
+      if (reset==0) stopall(); /* Kill all momentum. */
+    }
+    /* The stops work much better with smaller time steps */
+    for (i=0;i<16;++i) {
+      dSpaceCollide (space,0,&nearCallback);
+      /* Either stepper seems to work fine. */
+      //dWorldStep (world,0.001); 
+      dWorldQuickStep(world, 0.001);
+      /* remove all contact joints */
+      dJointGroupEmpty (contactgroup);
+    }
+    if (lCntDown>0) {
+      lCntDown-=1;
+      if (lCntDown==0) {
+        for (i=0;i<FLIPNUM;++i) {
+          dJointSetHingeParam(lFlipHinge[i],dParamVel, -5);
+        }
+      }
+    }
+    if (rCntDown>0) {
+      rCntDown-=1;
+      if (rCntDown==0) {
+        for (i=0;i<FLIPNUM;++i) {
+          dJointSetHingeParam(rFlipHinge[i],dParamVel,  5);
+        }
+      }
+    }
+
+  }
+
+  dsSetColor (1,1,0);
+  dsSetTexture (DS_WOOD);
+
+  /* Draw the balls */
+  for (i=0;i<SPHERENUM;++i) {
+    dsDrawSphere(dBodyGetPosition(sphereBody[i]),dBodyGetRotation(sphereBody[i]),sphereRad);
+  }
+
+  /* Draw the flippers */
+  dsSetColor(1,.5f,0);
+  for (i=0;i<FLIPNUM;++i) {
+    dsDrawBox(dBodyGetPosition(lFlipper[i]),dBodyGetRotation(lFlipper[i]),flipDim);
+    dsDrawBox(dBodyGetPosition(rFlipper[i]),dBodyGetRotation(rFlipper[i]),flipDim);
+  }
+
+  dsSetColor(1,0,1);
+  for (i=0;i<PADNUM;++i) {
+    dsDrawBox(dBodyGetPosition(paddle[i]),dBodyGetRotation(paddle[i]),padDim);
+  }
+
+  dsSetColor(0,.5f,0);
+  for (i=0;i<SPINNUM;++i) {
+    dsDrawBox(dBodyGetPosition(spinner[i]),dBodyGetRotation(spinner[i]),spinDim);
+  }
+
+  /*dsSetColor(0.2f,.2f,0.2f);
+  dsDrawBox(dBodyGetPosition(popper),dBodyGetRotation(popper),popDim);*/
+  
+
+  /* Draw the game plane. */
+  dsSetColor(0,0,1);
+  dsDrawBox(dBodyGetPosition(planeBody),dBodyGetRotation(planeBody),planeDim);
+}
+
+void stopBody(dBodyID b)
+{
+  dBodySetLinearVel(b,0,0,0);
+  dBodySetAngularVel(b,0,0,0);
+}
+
+void stopall()
+{
+  int i;
+  stopBody(planeBody);
+  for (i=0;i<FLIPNUM;++i) {
+    stopBody(lFlipper[i]);
+    stopBody(rFlipper[i]);
+  }
+  for (i=0;i<SPINNUM;++i) {
+    stopBody(spinner[i]);
+  }
+  for (i=0;i<PADNUM;++i) {
+    stopBody(paddle[i]);
+  }
+}
+
+void command(int c)
+{
+  int i;
+  switch (c) {
+    case 's':
+      stopall();
+
+      break;
+    case 'z':
+      for (i=0;i<FLIPNUM;++i) {
+        dJointAddHingeTorque(lFlipHinge[i],500);
+        dJointSetHingeParam(lFlipHinge[i],dParamVel,20);
+        lCntDown=30;
+      }
+      break;
+    case '/':
+      for (i=0;i<FLIPNUM;++i) {
+        dJointAddHingeTorque(rFlipHinge[i],-500);
+        dJointSetHingeParam(rFlipHinge[i],dParamVel, -20);
+        rCntDown=30;
+      }
+      break;
+    case ' ':
+      dJointAddAMPlusTorque(planeLimit,0,-500000);
+      //dJointAddAMPlusTorque(popAMP,1,1000);
+      break;
+  }
+}
+
+void rotateBody(dBodyID b,dVector3 ax,dReal ang)
+{
+  dQuaternion qAdd;
+  dQuaternion qReplace;
+  const dReal* qBody;
+  
+  dQFromAxisAndAngle(qAdd,ax[0],ax[1],ax[2],ang);
+  qBody = dBodyGetQuaternion(b);
+
+  /* Unlike vector operations, quaternion multiplication
+     has not been made safe against aliasing; so we need
+     an extra variable here. */
+  dQMultiply0(qReplace,qAdd,qBody);
+  dBodySetQuaternion(b,qReplace);
+}
+
+int main (int argc, char **argv)
+{
+  dMass m;
+  int i;
+  dVector3 yAxis={0,1,0,0};
+  dVector3 zAxis={0,0,1,0};
+
+  /* setup pointers to drawstuff callback functions */
+  dsFunctions fn;
+  fn.version = DS_VERSION;
+  fn.start = &start;
+  fn.step = &simLoop;
+  fn.command = &command;
+  fn.stop = 0;
+  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
+
+  /* create world */
+  dInitODE2(0);
+  world = dWorldCreate();
+  space = dHashSpaceCreate (0);
+  contactgroup = dJointGroupCreate (0);
+  dWorldSetGravity (world,0,0,-10);
+  dWorldSetQuickStepNumIterations(world, 50);
+  dCreatePlane (space,0,0,1,0); /* Ground plane*/
+  dWorldSetMaxAngularSpeed(world,25);
+
+
+  /* We create a big flat box.  */
+  dMassSetBox(&m,DENSITY/4,planeDim[0],planeDim[1],planeDim[2]);
+  planeBody = dBodyCreate(world);
+  dBodySetMass(planeBody,&m);
+  dGeomSetBody(dCreateBox(space,planeDim[0],planeDim[1],planeDim[2]),planeBody);
+  dBodySetPosition(planeBody,0,0,1);
+  planeAnchor = dJointCreateBall(world,0);
+  dJointAttach(planeAnchor,planeBody,0);
+  dJointSetBallAnchor(planeAnchor,0,0,1);
+  planeLimit = dJointCreateAMPlus(world,0);
+  dJointAttach(planeLimit,planeBody,0);
+  dJointSetAMPlusAxis1Rel(planeLimit,0,0,0,1);
+  dJointSetAMPlusAxis2Rel(planeLimit,0,0,0,1);
+  dJointSetAMPlusParam(planeLimit,dParamHiStop,M_PI/24);
+  //dJointSetAMPlusParam(planeLimit,dParamVel,0);
+  //dJointSetAMPlusParam(planeLimit,dParamFMax,10000);
+
+  /*dJointSetAMPlusAxis1Rel(planeLimit,1,1,0,0); 
+  dJointSetAMPlusAxis2Rel(planeLimit,1,1,0,0);
+  dJointSetAMPlusAxisRefRel(planeLimit,1,2,0,0,1);
+  dJointSetAMPlusParam(planeLimit,dParamFMax2,10);
+  dJointSetAMPlusParam(planeLimit,dParamVel2,0);
+  dJointSetAMPlusMode(planeLimit,1,dAMPlusProjected);
+  dJointSetAMPlusParam(planeLimit,dParamLoStop2,-M_PI/4);
+  dJointSetAMPlusParam(planeLimit,dParamHiStop2,M_PI/4);*/
+  
+  /* We create a bunch of paddles. */
+  dMassSetBox(&m,DENSITY/4,flipDim[0],flipDim[1],flipDim[2]);
+  for (i=0;i<FLIPNUM;++i) {
+    lFlipper[i] = dBodyCreate(world);
+    rFlipper[i] = dBodyCreate(world);
+    dBodySetMass(lFlipper[i],&m);
+    dBodySetMass(rFlipper[i],&m);
+    dGeomSetBody(dCreateBox(space,flipDim[0],flipDim[1],flipDim[2]),lFlipper[i]);
+    dGeomSetBody(dCreateBox(space,flipDim[0],flipDim[1],flipDim[2]),rFlipper[i]);
+    lFlipHinge[i] = dJointCreateHinge(world,0);
+    rFlipHinge[i] = dJointCreateHinge(world,0);
+    dJointAttach(lFlipHinge[i],lFlipper[i],planeBody);
+    dJointAttach(rFlipHinge[i],rFlipper[i],planeBody);
+
+    dBodySetPosition(lFlipper[i],
+      -flipDim[0]/2-sphereRad,
+      -planeDim[1]/2-3*flipDim[1]/4,
+      1+flipDim[2]/2+planeDim[2]/2);
+    dBodySetPosition(rFlipper[i],
+       flipDim[0]/2+sphereRad,
+      -planeDim[1]/2-3*flipDim[1]/4,
+      1+flipDim[2]/2+planeDim[2]/2);
+    dJointSetHingeAnchor(lFlipHinge[i],
+      -flipDim[0],
+      -planeDim[1]/2,
+      1+flipDim[2]/2);
+    dJointSetHingeAnchor(rFlipHinge[i],
+       flipDim[0],
+      -planeDim[1]/2,
+      1+flipDim[2]/2+planeDim[2]);
+    dJointSetHingeAxis(lFlipHinge[i],0,0,1);
+    dJointSetHingeAxis(rFlipHinge[i],0,0,1);
+    dJointSetHingeParam(lFlipHinge[i],dParamLoStop,0);
+    dJointSetHingeParam(rFlipHinge[i],dParamLoStop,-M_PI/4);
+    dJointSetHingeParam(lFlipHinge[i],dParamHiStop,M_PI/4);
+    dJointSetHingeParam(rFlipHinge[i],dParamHiStop,0);
+    dJointSetHingeParam(lFlipHinge[i],dParamFMax,200);
+    dJointSetHingeParam(rFlipHinge[i],dParamFMax,200);
+    dJointSetHingeParam(lFlipHinge[i],dParamVel,-5);
+    dJointSetHingeParam(rFlipHinge[i],dParamVel, 5);
+
+    rotateBody(planeBody,zAxis,M_PI/2);
+  }
+
+  /* Create the balls */
+  dMassSetSphere(&m,DENSITY,sphereRad);
+  for (i = 0; i<SPHERENUM; ++i) {
+    sphereBody[i] = dBodyCreate(world);
+    dBodySetMass(sphereBody[i],&m);
+    dGeomSetBody( dCreateSphere(space,sphereRad), sphereBody[i]);
+    sphereJoint[i] = dJointCreateLMPlus(world,0);
+    dJointAttach(sphereJoint[i],sphereBody[i],planeBody);
+    dJointSetLMPlusAxisRel(sphereJoint[i],0,2,1,0,0);
+    dJointSetLMPlusAxisRel(sphereJoint[i],1,2,0,1,0);
+    dJointSetLMPlusAxisRel(sphereJoint[i],2,2,0,0,1);
+    dJointSetLMPlusParam(sphereJoint[i],dParamLoStop1,-planeDim[0]/2);
+    dJointSetLMPlusParam(sphereJoint[i],dParamHiStop1, planeDim[0]/2);
+    dJointSetLMPlusParam(sphereJoint[i],dParamLoStop2,-planeDim[1]/2);
+    dJointSetLMPlusParam(sphereJoint[i],dParamHiStop2, planeDim[1]/2);
+    dJointSetLMPlusParam(sphereJoint[i],dParamLoStop3, 0);
+    dJointSetLMPlusParam(sphereJoint[i],dParamHiStop3, planeDim[2]/2+2);
+  }
+
+  dMassSetBox(&m,DENSITY/4,spinDim[0],spinDim[1],spinDim[2]);
+  for (i=0;i<SPINNUM;++i) {
+    spinner[i] = dBodyCreate(world);
+    dBodySetMass(spinner[i],&m);
+    dGeomSetBody(dCreateBox(space,spinDim[0],spinDim[1],spinDim[2]),spinner[i]);
+    //dBodySetPosition(spinner[i],0,i*spinDim[1],3+planeDim[2]/2+spinDim[0]/2);
+    
+    spinAMP[i] = dJointCreateAMPlus(world,0);
+    dJointAttach(spinAMP[i],spinner[i],planeBody);
+    rotateBody(spinner[i],yAxis,M_PI/2);
+    dJointSetAMPlusUniversal(spinAMP[i],1,0,0, 0,0,1);
+
+    spinLMP[i] = dJointCreateLMPlus(world,0);
+    dJointAttach(spinLMP[i],spinner[i],planeBody);
+    dJointSetLMPlusAnchor1Rel(spinLMP[i],0,0,0);
+    dJointSetLMPlusAnchor2Rel(spinLMP[i],0,0,0);
+    dJointSetLMPlusAxis(spinLMP[i],0,2,1,0,0);
+    dJointSetLMPlusAxis(spinLMP[i],1,2,0,1,0);
+    dJointSetLMPlusAxis(spinLMP[i],2,2,0,0,1);
+    dJointSetLMPlusParam(spinLMP[i],dParamLoStop1,-planeDim[0]/2);
+    dJointSetLMPlusParam(spinLMP[i],dParamHiStop1, planeDim[0]/2);
+    dJointSetLMPlusParam(spinLMP[i],dParamLoStop2,-planeDim[1]/2);
+    dJointSetLMPlusParam(spinLMP[i],dParamHiStop2, planeDim[1]/2);
+    dJointSetLMPlusParam(spinLMP[i],dParamLoStop3, planeDim[2]/2 + spinDim[0]/2);
+    dJointSetLMPlusParam(spinLMP[i],dParamHiStop3, planeDim[2]/2 + spinDim[0]/2);
+  }
+
+  dMassSetBox(&m,DENSITY/4,padDim[0],padDim[1],padDim[2]);
+  for (i=0;i<PADNUM;++i) {
+    paddle[i] = dBodyCreate(world);
+    dBodySetMass(paddle[i],&m);
+    dGeomSetBody(dCreateBox(space,padDim[0],padDim[1],padDim[2]),paddle[i]);
+    //dBodySetPosition(paddle[i],i*padDim[2],1,3+planeDim[2]/2+padDim[0]);
+    
+    padAMP[i] = dJointCreateAMPlus(world,0);
+    dJointAttach(padAMP[i],paddle[i],planeBody);
+    rotateBody(paddle[i],yAxis,M_PI/2);
+    dJointSetAMPlusHinge(padAMP[i],1,0,0);
+
+    padLMP[i] = dJointCreateLMPlus(world,0);
+    dJointAttach(padLMP[i],paddle[i],planeBody);
+    dJointSetLMPlusAnchor1Rel(padLMP[i],padDim[0]/2,0,0);
+    dJointSetLMPlusAnchor2Rel(padLMP[i],0,0,0);
+    dJointSetLMPlusAxis(padLMP[i],0,2,1,0,0);
+    dJointSetLMPlusAxis(padLMP[i],1,2,0,1,0);
+    dJointSetLMPlusAxis(padLMP[i],2,2,0,0,1);
+    dJointSetLMPlusParam(padLMP[i],dParamLoStop1,-planeDim[0]/2);
+    dJointSetLMPlusParam(padLMP[i],dParamHiStop1, planeDim[0]/2);
+    dJointSetLMPlusParam(padLMP[i],dParamLoStop2,-planeDim[1]/2);
+    dJointSetLMPlusParam(padLMP[i],dParamHiStop2, planeDim[1]/2);
+    dJointSetLMPlusParam(padLMP[i],dParamLoStop3, planeDim[2]/2 + padDim[0]);
+    dJointSetLMPlusParam(padLMP[i],dParamHiStop3, planeDim[2]/2 + padDim[0]);
+  }
+
+  /* We create a popper.  */
+  //dMassSetBox(&m,DENSITY/4,popDim[0],popDim[1],popDim[2]);
+  //popper = dBodyCreate(world);
+  //dBodySetMass(popper,&m);
+  //dGeomSetBody(dCreateBox(space,popDim[0],popDim[1],popDim[2]),popper);
+
+  //popAMP = dJointCreateAMPlus(world,0);
+  //dJointAttach(popAMP,popper,planeBody);
+  ////dJointSetAMPlusHinge(popAMP,1,0,0);
+  //dJointSetAMPlusUniversal(popAMP,1,0,0, 0,0,1);
+  //dJointSetAMPlusParam(popAMP,dParamLoStop2,-0.001);
+  //dJointSetAMPlusParam(popAMP,dParamHiStop2,M_PI/2);
+
+  //popLMP = dJointCreateLMPlus(world,0);
+  //dJointAttach(popLMP,popper,planeBody);
+  //dJointSetLMPlusAnchor1Rel(popLMP,0.01,popDim[1]/2,popDim[2]/2);
+  //dJointSetLMPlusAnchor2Rel(popLMP,0,0,planeDim[2]/2);
+  //dJointSetLMPlusAxis(popLMP,0,2,1,0,0);
+  //dJointSetLMPlusAxis(popLMP,1,2,0,1,0);
+  //dJointSetLMPlusAxis(popLMP,2,2,0,0,1);
+  //dJointSetLMPlusParam(popLMP,dParamLoStop1,-planeDim[0]/3);
+  //dJointSetLMPlusParam(popLMP,dParamHiStop1, planeDim[0]/3);
+  //dJointSetLMPlusParam(popLMP,dParamLoStop2,-planeDim[1]/3);
+  //dJointSetLMPlusParam(popLMP,dParamHiStop2, planeDim[1]/3);
+  //dJointSetLMPlusParam(popLMP,dParamLoStop3, 0.01);
+  //dJointSetLMPlusParam(popLMP,dParamHiStop3, 0.01);
+
+  
+  /* run simulation */
+  dsSimulationLoop (argc,argv,352,288,&fn);
+
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+  dCloseODE();
+  return 0;
+}
Index: ode/demo/demo_lmplus.cpp
===================================================================
--- ode/demo/demo_lmplus.cpp	(revision 0)
+++ ode/demo/demo_lmplus.cpp	(working copy)
@@ -0,0 +1,486 @@
+/*************************************************************************
+ * LMPlus demo, written for the                                          *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ *   This demo was built from demo_chain1.c                              *
+ *   Content novel to this file Copyright 2012, Joseph Cooper            *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * Novel material in this file can also, at your option, be distributed  *
+ * under the ZLib license,                                               *
+ * see http://www.gzip.org/zlib/zlib_license.html                        *
+ *                                                                       *
+ * 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 and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+/* exercise the C interface */
+
+#include <stdio.h>
+#include "ode/ode.h"
+#include "drawstuff/drawstuff.h"
+#include "texturepath.h"
+
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+
+/* select correct drawing functions */
+
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCapsule dsDrawCapsuleD
+#define dsDrawLine dsDrawLineD
+#endif
+
+
+/* some constants */
+#define DNUM 4     /* number of dominoes */
+#define S5NUM (3)  /* Bodies in the sphere volume */
+#define S4NUM (3)  /* Bodies in the cylinder volume */
+#define S3NUM (3)  /* Bodies in the box volume */
+#define S2NUM (3)  /* Bodies in the plane */
+#define S1NUM (1)  /* Bodies on the line */
+#define SNUM (S5NUM + S4NUM + S3NUM + S2NUM + S1NUM)	/* number of spheres */
+#define RADIUS (0.15f)	/* sphere radius */
+#define DENSITY (1000.0f) 
+
+/* dynamics and collision objects */
+static dWorldID world;
+static dSpaceID space;
+static dJointGroupID contactgroup;
+
+/* A wandering kinematic sphere */
+static dBodyID kSphereBody;
+static dReal kSphereDim = 0.5;
+static dVector3 kSphereOrigin = {-1,-4,3};
+
+/* A spinning kinematic cylinder */
+static dBodyID cylBody;
+static dReal cylDim[2] = {2,0.5};
+static dVector3 cylOrigin = {4,-2,3};
+
+/* A spinning kinematic box */
+static dBodyID boxBody;
+static dReal boxDim[3] = {2,2,2};
+
+/* A rotating kinematic 'plane' */
+static dBodyID planeBody;
+static dReal planeDim[3] = {2,2,.05};
+
+/* Flippers that can push balls around */
+static dBodyID dominoBody[DNUM];
+static dReal domDim[3] = {.05,.4,.8};
+
+/* A bunch of balls */
+static dBodyID sphereBody[SNUM];
+static dJointID sJoint[SNUM];
+
+
+/* this is called by dSpaceCollide when two objects in space are
+ * potentially colliding.
+ */
+
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+{
+  /* exit without doing anything if the two bodies are connected by a joint */
+  dBodyID b1,b2;
+  dContact contact;
+
+  b1 = dGeomGetBody(o1);
+  b2 = dGeomGetBody(o2);
+  /* Even if objects are connected by a genLinear, we still
+     want them to collide */
+  if (b1 && b2 && dAreConnectedExcluding( b1,b2,dJointTypeLMPlus)) return;
+
+  contact.surface.mode = 0;
+  contact.surface.mu = 0.1;
+  contact.surface.mu2 = 0;
+  if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
+    dJointID c = dJointCreateContact (world,contactgroup,&contact);
+    dJointAttach (c,b1,b2);
+  }
+}
+
+
+/* start simulation - set viewpoint */
+
+static void start()
+{
+  static float xyz[3] = {2.1640f,-5.3079f,2.7600f};
+  static float hpr[3] = {110.0000f,0.0000f,0.0000f};
+
+  dAllocateODEDataForThread(dAllocateMaskAll);
+  dsSetViewpoint (xyz,hpr);
+}
+
+/* simulation loop */
+
+static void simLoop (int pause)
+{
+  int i,j;
+  const dReal* ballPos;
+	const dReal* ksVel;
+	const dReal* ksPos;
+	dReal ksDist;
+  dVector3 anchorPos;
+  dVector3 endPos;
+	dVector3 newVel;
+  static dReal tv = 0;
+  static int reset = 1;
+
+  if (!pause) {
+    /* The stops work much better with smaller time steps */
+    for (i=0;i<16;++i) {
+      dSpaceCollide (space,0,&nearCallback);
+      /* Either stepper seems to work fine. */
+      dWorldStep (world,0.001); 
+      //dWorldQuickStep(world, 0.001);
+
+      /* remove all contact joints */
+      dJointGroupEmpty (contactgroup);
+    }
+    if (reset) {
+      /* Since we create all of the bodies in 
+         unsatisfied states, we kill the momentum 
+         after ERP has fixed that.
+      */
+      
+      for (i=0;i<SNUM;++i) {
+			  dBodySetAngularVel(sphereBody[i],0,0,0);
+			  dBodySetLinearVel(sphereBody[i],0,0,0);
+		  }
+      reset = 0;
+    }
+
+    /* Push the linearly constrained ball around. */
+    tv += M_PI/200;
+    if (tv>=2*M_PI) tv=0;
+    dJointSetLMPlusParam(sJoint[0],dParamVel, dSin(tv)*2 );
+		
+		// Make the sphere wander randomly to shake
+		// the attached bodies around.
+		ksPos = dBodyGetPosition(kSphereBody);
+		ksDist = dCalcPointsDistance3(ksPos,kSphereOrigin);
+		if (ksDist>=1.5) {
+			dSubtractVectors3(newVel,kSphereOrigin,ksPos);
+			dScaleVector3(newVel,0.25/ksDist);
+		} else {
+			ksVel = dBodyGetLinearVel(kSphereBody);
+			for (j=0;j<3;++j) {
+				newVel[j]=ksVel[j] + (dRandReal()-0.5)*0.2;
+			}
+		}
+		dBodySetLinearVel(kSphereBody,newVel[0],newVel[1],newVel[2]);
+  }
+
+  dsSetColor (1,1,0);
+  dsSetTexture (DS_WOOD);
+
+  /* Draw all the balls */
+  for (i=0;i<SNUM;++i) {
+    ballPos = dBodyGetPosition(sphereBody[i]);
+    dJointGetLMPlusAnchor1( sJoint[i], anchorPos );
+    dsDrawLine(ballPos,anchorPos);
+    dsDrawSphere(dBodyGetPosition(sphereBody[i]),dBodyGetRotation(sphereBody[i]),RADIUS);
+  }
+
+  /* Draw the flippers */
+  dsSetColor(1,.5f,0);
+  for (i=0;i<DNUM;++i) {
+    dsDrawBox(dBodyGetPosition(dominoBody[i]),dBodyGetRotation(dominoBody[i]),domDim);
+  }
+
+  /* Draw the planar constraint box. */
+  dsSetColor(0,0,1);
+  dsDrawBox(dBodyGetPosition(planeBody),dBodyGetRotation(planeBody),planeDim);
+
+  /* Draw the constraint line */ 
+  anchorPos[0]=3; endPos[0] = 3;
+  anchorPos[1]=0; endPos[1] = 0;
+  anchorPos[2]=1; endPos[2] = 3;
+  dsDrawLine(anchorPos,endPos);
+
+  /* Draw the volume constraints last because of alpha. */
+  /* Use alpha because they have no geom and the balls
+		 can fall through. */
+  dsSetColorAlpha(0,0,1,.75);
+  dsDrawSphere(dBodyGetPosition(kSphereBody),dBodyGetRotation(kSphereBody),kSphereDim);
+	dsDrawBox(dBodyGetPosition(boxBody),dBodyGetRotation(boxBody),boxDim);
+	dsDrawCylinder(dBodyGetPosition(cylBody),dBodyGetRotation(cylBody),cylDim[0],cylDim[1]);
+}
+
+void command(int c)
+{
+	int ii;
+  switch (c) {
+    case ' ':
+			for (ii=S1NUM+S2NUM+S3NUM+S4NUM;ii<SNUM;++ii) {
+				dBodySetAngularVel(sphereBody[ii],0,0,0);
+				dBodySetLinearVel(sphereBody[ii],0,0,0);
+			}
+      break;
+  }
+}
+
+int main (int argc, char **argv)
+{
+  dMass m;
+  dMatrix3 rMat;
+  int i;
+  dJointID tmpJoint;
+
+  /* setup pointers to drawstuff callback functions */
+  dsFunctions fn;
+  fn.version = DS_VERSION;
+  fn.start = &start;
+  fn.step = &simLoop;
+  fn.command = command;
+  fn.stop = 0;
+  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
+
+  /* create world */
+  dInitODE2(0);
+  world = dWorldCreate();
+  space = dHashSpaceCreate (0);
+  contactgroup = dJointGroupCreate (1000000);
+  dWorldSetGravity (world,0,0,-10);
+  dWorldSetQuickStepNumIterations(world, 50);
+	//dWorldSetMaxAngularSpeed(world,10);
+	//dWorldSetAngularDamping(world,0.001);
+  dCreatePlane (space,0,0,1,0);
+
+	/* Create a kinematic body for the sphere volume constraint. */
+	kSphereBody = dBodyCreate(world);
+	dBodySetKinematic(kSphereBody);
+	dBodySetPosition(kSphereBody,kSphereOrigin[0],kSphereOrigin[1],kSphereOrigin[2]);
+
+	/* Create a kinematic body for the cylinder volume constraint. */
+	cylBody = dBodyCreate(world);
+	dBodySetKinematic(cylBody);
+	dBodySetPosition(cylBody,4,-2,3);
+
+
+  /* Create a kinematic body for the box volume constraint. */
+  boxBody = dBodyCreate(world);
+  dBodySetKinematic(boxBody);
+  /* No geom so objects can fall through. */
+  /*dGeomSetBody(dCreateBox(space,boxDim[0],boxDim[1],boxDim[2]),boxBody);*/
+  dBodySetPosition(boxBody,1,1,3);
+  
+  /* Kinematic body for the plane constraint. */
+  planeBody = dBodyCreate(world);
+  dBodySetKinematic(planeBody);
+  dGeomSetBody(dCreateBox(space,planeDim[0],planeDim[1],planeDim[2]),planeBody);
+  dBodySetPosition(planeBody,-2,-1,3);
+  
+
+  /* Create a couple flippers. */
+  dMassSetBox(&m,DENSITY,domDim[0],domDim[1],domDim[2]);
+  for (i = 0; i<DNUM;++i) {
+    dominoBody[i] = dBodyCreate(world);
+    dBodySetMass(dominoBody[i],&m);
+    dGeomSetBody(dCreateBox(space,domDim[0],domDim[1],domDim[2]),dominoBody[i] );
+    /* Position all of the dominoes as desired. */
+    dBodySetPosition(dominoBody[i],2.6+(i%2)*0.8,0,1+i*0.5);
+    tmpJoint = dJointCreateHinge(world,0);
+    dJointAttach(tmpJoint,dominoBody[i],0);
+    dJointSetHingeAnchor(tmpJoint,2.6+(i%2)*0.8,0,1+i*0.5);
+    dJointSetHingeAxis(tmpJoint,0,1,0);
+    dJointSetHingeParam(tmpJoint,dParamVel,0.75);
+    dJointSetHingeParam(tmpJoint,dParamFMax,75);
+  }
+ 
+  /* Create the balls */
+  dMassSetSphere(&m,DENSITY,RADIUS);
+  for (i = 0; i<SNUM; ++i) {
+    sphereBody[i] = dBodyCreate(world);
+    dBodySetMass(sphereBody[i],&m);
+    dGeomSetBody( dCreateSphere(space,RADIUS), sphereBody[i]);
+
+    
+  }
+
+	int kk=0;
+	/* Assign sphere to the linear constraint.  */
+  for (i = kk; i<(kk+S1NUM); ++i) {
+    sJoint[i] = dJointCreateLMPlus(world,0);
+    dJointAttach (sJoint[i],sphereBody[i],0);
+    /* Make the constraints relative to the world */
+    dJointSetLMPlusMode(sJoint[i],0,dLMPlusPlanar); 
+		dJointSetLMPlusMode(sJoint[i],1,dLMPlusPlanar); 
+		dJointSetLMPlusMode(sJoint[i],2,dLMPlusPlanar); 
+    dJointSetLMPlusAxis(sJoint[i],0, 0, 0,0,1);
+    dJointSetLMPlusAxis(sJoint[i],1, 0, 1,0,0);
+    dJointSetLMPlusAxis(sJoint[i],2, 0, 0,1,0);
+    /* All axes are limits keeping the ref. point
+       of the ball inside the box. */
+    dJointSetLMPlusAnchor1Rel(sJoint[i],0,0,0.8);
+    dJointSetLMPlusAnchor2Rel(sJoint[i],3,0,2);
+    /* We need a less verbose way of setting the parameters... */    
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop1, -1);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop1, 1);
+    dJointSetLMPlusParam(sJoint[i],dParamFMax1, 350 ); /* Turn on the motor. */
+    dJointSetLMPlusParam(sJoint[i],dParamVel1, 0 );
+		dJointSetLMPlusParam(sJoint[i],dParamLoStop2, 0);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop2, 0);
+		dJointSetLMPlusParam(sJoint[i],dParamLoStop3, 0);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop3, 0);
+    
+    dBodySetPosition(sphereBody[i], 3,0,dRandReal()*2+1);
+  }
+	kk+=S1NUM;
+
+	/* Assign some balls to the planar constraint. */
+  for (i = kk; i<(S2NUM+kk); ++i) {
+    sJoint[i] = dJointCreateLMPlus(world,0);
+    dJointAttach (sJoint[i],sphereBody[i],planeBody);
+    /* Make the constraints relative to the box */
+    dJointSetLMPlusMode(sJoint[i],0,dLMPlusPlanar);
+		dJointSetLMPlusMode(sJoint[i],1,dLMPlusPlanar);
+		dJointSetLMPlusMode(sJoint[i],2,dLMPlusPlanar);
+    dJointSetLMPlusAxis(sJoint[i],0, 2, 0,0,1);
+    dJointSetLMPlusAxis(sJoint[i],1, 2, 1,0,0);
+    dJointSetLMPlusAxis(sJoint[i],2, 2, 0,1,0);
+    
+    dJointSetLMPlusAnchor1Rel(sJoint[i],0,0,0.8);
+    dJointSetLMPlusAnchor2Rel(sJoint[i],0,0,0);
+    /* We need a less verbose way of setting the parameters... */    
+		dJointSetLMPlusParam(sJoint[i],dParamLoStop1, 0);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop1, 0);
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop2,-1);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop2, 1);
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop3,-1);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop3, 1);
+
+    dBodySetPosition(sphereBody[i],-dRandReal()*2-1,-dRandReal()*2,1);
+  }
+	kk+=S2NUM;
+  
+  /* Assign some spheres to the box volume constraint.
+     We're sloppy with positioning them.  The first
+     few frames of simulation should fix the error. */ 
+  for (i = kk; i<(S3NUM+kk); ++i) {
+    sJoint[i] = dJointCreateLMPlus(world,0);
+    dJointAttach (sJoint[i],sphereBody[i],boxBody);
+
+		/* Put the constraint in planar mode */
+		dJointSetLMPlusMode(sJoint[i],0,dLMPlusPlanar);
+		dJointSetLMPlusMode(sJoint[i],1,dLMPlusPlanar);
+		dJointSetLMPlusMode(sJoint[i],2,dLMPlusPlanar);
+		/* Make the constraints relative to the box */
+    dJointSetLMPlusAxis(sJoint[i],0, 2, 0,0,1);
+		dJointSetLMPlusAxis(sJoint[i],1, 2, 1,0,0);
+		dJointSetLMPlusAxis(sJoint[i],2, 2, 0,1,0);
+    
+    /* Set the reference points on the two bodies. */
+    dJointSetLMPlusAnchor1Rel(sJoint[i],0,0,0.8);
+		dJointSetLMPlusAnchor2Rel(sJoint[i],0,0,0);
+    
+		/* All axes are limits keeping the ref. point
+       of the ball inside the box. */
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop1,-1);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop1, 1);
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop2,-1);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop2, 1);
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop3,-1);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop3, 1);
+
+    dBodySetPosition(sphereBody[i],dRandReal()*2,dRandReal()*2,1);
+  }
+	kk+=S3NUM;
+
+	/* Assign some spheres to the cylinder volume constraint. */ 
+  for (i = kk; i<(S4NUM+kk); ++i) {
+    sJoint[i] = dJointCreateLMPlus(world,0);
+    dJointAttach (sJoint[i],sphereBody[i],cylBody);
+
+		/* Put one constraint in planar mode */
+		dJointSetLMPlusMode(sJoint[i],0,dLMPlusPlanar);
+		/* And one constraint in cylinder mode */
+		dJointSetLMPlusMode(sJoint[i],1,dLMPlusCylindrical);
+		
+		/* Make the constraints relative to the volume */
+    dJointSetLMPlusAxis(sJoint[i],0, 2, 0,0,1);
+		dJointSetLMPlusAxis(sJoint[i],1, 2, 0,0,1);
+    
+    /* Set the reference points on the two bodies. */
+    dJointSetLMPlusAnchor1Rel(sJoint[i],0,0,0.8);
+		dJointSetLMPlusAnchor2Rel(sJoint[i],0,0,0);
+    
+		/* All axes are limits keeping the ref. point
+       of the ball inside the box. */
+    dJointSetLMPlusParam(sJoint[i],dParamLoStop1,-cylDim[0]/2.0);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop1, cylDim[0]/2.0);
+    //dJointSetLMPlusParam(sJoint[i],dParamLoStop2, cylDim[1]/2.0);
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop2, cylDim[1]);
+
+    dBodySetPosition(sphereBody[i],cylOrigin[0]+dRandReal()-0.5,cylOrigin[1]+dRandReal()-0.5,cylOrigin[2]);
+  }
+	kk+=S4NUM;
+
+	/* Assign some balls to the sphere volume constraint. */ 
+  for (i = kk; i<(S5NUM+kk); ++i) {
+    sJoint[i] = dJointCreateLMPlus(world,0);
+    dJointAttach (sJoint[i],sphereBody[i],kSphereBody);
+
+		/* Put the constraint in planar mode */
+		dJointSetLMPlusMode(sJoint[i],0,dLMPlusSpherical);
+    
+    /* Set the reference points on the two bodies. */
+    dJointSetLMPlusAnchor1Rel(sJoint[i],0,0,0.8);
+		dJointSetLMPlusAnchor2Rel(sJoint[i],0,0,0);
+    
+		/* The joint limit keeps the ref. point
+       of the small balls inside the other ball. */
+    dJointSetLMPlusParam(sJoint[i],dParamHiStop1,kSphereDim);
+		//dJointSetLMPlusParam(sJoint[i],dParamStopCFM,0.00001);
+		dJointSetLMPlusParam(sJoint[i],dParamStopERP,0.1);
+
+		/* Add a motor constraint pulling the points together. */
+		//dJointSetLMPlusParam(sJoint[i],dParamVel,-0.25);
+		//dJointSetLMPlusParam(sJoint[i],dParamFMax,100);
+
+		dBodySetPosition(sphereBody[i],
+			kSphereOrigin[0]+kSphereDim*dCos(2*M_PI/i),
+			kSphereOrigin[1]+kSphereDim*dSin(2*M_PI/i),
+			kSphereOrigin[2]);
+
+  }
+	kk+=S5NUM;
+	
+
+  /* Axes are set in global frame. Make sure the body's not
+     rotated until they're set. */
+  dRFromEulerAngles(rMat,M_PI/4,M_PI/4,0);
+  dBodySetRotation(boxBody,rMat);
+
+
+	dBodySetAngularVel(boxBody,0,1,0.15);
+	dBodySetAngularVel(cylBody,0,.75,0);
+	dBodySetAngularVel(planeBody,.50,0,0.05);
+
+  /* run simulation */
+  dsSimulationLoop (argc,argv,352,288,&fn);
+
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+  dCloseODE();
+  return 0;
+}
Index: ode/demo/Makefile.am
===================================================================
--- ode/demo/Makefile.am	(revision 1903)
+++ ode/demo/Makefile.am	(working copy)
@@ -36,6 +36,8 @@
                 demo_kinematic \
                 demo_motion \
                 demo_motor \
+                demo_amplus \
+                demo_lmplus \
                 demo_ode \
                 demo_piston \
                 demo_plane2d \
@@ -70,6 +72,8 @@
 demo_kinematic_SOURCES = demo_kinematic.cpp
 demo_motion_SOURCES = demo_motion.cpp
 demo_motor_SOURCES = demo_motor.cpp
+demo_amplus_SOURCES = demo_amplus.cpp
+demo_lmplus_SOURCES = demo_lmplus.cpp
 demo_ode_SOURCES = demo_ode.cpp
 demo_piston_SOURCES = demo_piston.cpp
 demo_plane2d_SOURCES = demo_plane2d.cpp
Index: ode/src/export-dif.cpp
===================================================================
--- ode/src/export-dif.cpp	(revision 1903)
+++ ode/src/export-dif.cpp	(working copy)
@@ -163,7 +163,9 @@
         c.print ("motor = {");
     }
     c.indent++;
-    c.printNonzero ("vel",limot.vel);
+    //c.printNonzero ("vel",limot.vel);
+    c.printNonzero ("lo_vel",limot.lo_vel);
+    c.printNonzero ("hi_vel",limot.hi_vel);
     c.printNonzero ("fmax",limot.fmax);
     c.print ("ODE = {");
     c.indent++;
Index: ode/src/joints/amotor.cpp
===================================================================
--- ode/src/joints/amotor.cpp	(revision 1903)
+++ ode/src/joints/amotor.cpp	(working copy)
@@ -184,7 +184,10 @@
 void 
 dxJointAMotor::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = num;
+    info->max_m = 0;
+    for (int i=0;i<num;++i) {
+      info->max_m += limot[i].countSureMaxRows();
+    }
 }
 
 
@@ -205,11 +208,9 @@
     // see if we're powered or at a joint limit for each axis
     for ( int i = 0; i < num; i++ )
     {
-        if ( limot[i].testRotationalLimit( angle[i] ) ||
-            limot[i].fmax > 0 )
-        {
-            info->m++;
-        }
+      limot[i].testRotationalLimit( angle[i] );
+      info->m += limot[i].countRows();
+      info->nub += limot[i].countUBRows();
     }
 }
 
@@ -256,7 +257,7 @@
     int row = 0;
     for ( i = 0; i < num; i++ )
     {
-        row += limot[i].addLimot( this, info, row, *( axptr[i] ), 1 );
+        row += limot[i].addRotationalLimot( this, info, *( axptr[i] ), row );
     }
 }
 
@@ -342,7 +343,7 @@
     if ( joint->mode == dAMotorUser )
     {
         if ( anum < 0 ) anum = 0;
-        if ( anum > 3 ) anum = 3;
+        if ( anum > 2 ) anum = 2;
         joint->angle[anum] = angle;
     }
 }
@@ -438,7 +439,7 @@
     dAASSERT( joint && anum >= 0 && anum < 3 );
     checktype( joint, AMotor );
     if ( anum < 0 ) anum = 0;
-    if ( anum > 3 ) anum = 3;
+    if ( anum > 2 ) anum = 2;
     return joint->angle[anum];
 }
 
Index: ode/src/joints/amplus.cpp
===================================================================
--- ode/src/joints/amplus.cpp	(revision 0)
+++ ode/src/joints/amplus.cpp	(working copy)
@@ -0,0 +1,570 @@
+/*************************************************************************
+ *                                                                       *
+ * AMPlus for Open Dynamics Engine.  Copyright (C) 2012 Joseph Cooper.   *
+ *                                                                       *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *   (3) The zlib/libpng license that is included with this library in   *
+ *       the file LICENSE-ZLIB.TXT                                       *
+ *                                                                       *
+ * This library is distributed WITHOUT ANY WARRANTY, including implied   *
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
+ * See the files LICENSE.TXT or LICENSE-BSD.TXT or LICENSE-ZLIB.TXT      *
+ *                                                                       *
+ *************************************************************************/
+
+
+#include "amplus.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// angular motor plus
+
+dxJointAMPlus::dxJointAMPlus( dxWorld *w ) 
+: dxJoint( w )
+{
+  for (int ii=0;ii<3;++ii) {
+    mode[ii] = dAMPlusCone;
+    frame[ii] = 2;
+    dSetZero(axis1[ii],4);
+    axis1[ii][2]=1;
+    dSetZero(axis2[ii],4);
+    axis2[ii][ii]=1;
+    dSetZero(axisRef[ii],4);
+    axisRef[ii][ii]=1;
+    limot[ii].init( world );
+  }
+}
+
+/**
+  Find the current angle and effective axis.
+*/
+dReal dxJointAMPlus::currentAngle(dVector3 axis,int anum)
+{
+  dReal angle=0;
+
+  switch (mode[anum]) {
+    case dAMPlusCone:
+    default: 
+    {
+      // Find the global axes
+      dVector3 ax1, ax2, tmp;
+      getAxis (this, ax1, axis1[anum]);
+      getAxis2(this, ax2, axis2[anum]);
+      // Find the angle between the two anchor axes
+      dReal dotp = dCalcVectorDot3(ax1,ax2);
+      // Even with unit length vectors, fp-error
+      // can push the dot product past one which 
+      // can crash acos().
+      if (dotp>1) dotp=1;  
+      if (dotp<-1) dotp=-1;
+      angle = dAcos(dotp);
+      if (angle!=0) {
+        dCalcVectorCross3(axis,ax2,ax1);
+        dNormalize3(axis);
+      } else {
+        // ***** Find the relative angular velocity
+        // of the bodies, use the orthogonal
+        // component of that if it's non-zero
+
+        // Otherwise, just use the first planespace axis.
+        dPlaneSpace(ax1,axis,tmp);
+      }
+    } break;
+    case dAMPlusProjected:
+    {
+      // Find the axes in global frame
+      dVector3 ax, axP, axQ, tmp;
+      if (frame[anum]==1) { 
+        // The reference axis is the axis of rotation
+        getAxis (this,axis,axisRef[anum]);
+        getAxis2(this, ax,axis2[anum]);
+        getAxis (this,tmp,axis1[anum]);
+        // ***** Things might be backwards here...
+        
+      } else {
+        getAxis2(this,axis,axisRef[anum]);
+        getAxis (this,  ax,axis1[anum]);
+        getAxis2(this, tmp,axis2[anum]);
+
+      }
+      // Find the planespace orthogonal to axis
+      dDirectedPlaneSpace(axis,tmp,axP,axQ);
+  
+      // Project the anchor axis onto the planespace
+      dReal dotp = dCalcVectorDot3(ax,axP);
+      dReal dotq = dCalcVectorDot3(ax,axQ);
+      if (dotp!=0 || dotq!=0) {
+        angle = dAtan2(dotq,dotp);
+      } else {
+        // ***** Should we set the angle to some special value? NaN?
+        // For now, we'll leave it at zero.
+      }
+      if (frame[anum]==1) {
+        dNegateVector3(axis);
+      }
+    } break;
+  }
+  return angle;
+}
+
+void 
+dxJointAMPlus::getSureMaxInfo( SureMaxInfo* info )
+{
+  // For each axis,
+  // If the axis is active +1
+  // If the axis has fudge disabled +1
+  // If the axis has bidirectional constraints enabled +1
+  info->max_m = 0;
+  for (int ii=0;ii<3;++ii) {
+    info->max_m += limot[ii].countSureMaxRows();
+  }
+}
+
+void
+dxJointAMPlus::getInfo1( dxJoint::Info1 *info )
+{
+  info->m = 0;
+  info->nub = 0;
+
+  // For each limot'ed axis
+	for (int ii=0;ii<3;++ii) {
+		if (limot[ii].isActive()) {
+			// Compute the axis/distance and cache them.
+			dReal angle = currentAngle(cacheAxis[ii],ii);
+      // The angle (error) is cached in the limot structure.
+			limot[ii].testRotationalLimit( angle );
+
+			info->m += limot[ii].countRows();
+			info->nub += limot[ii].countUBRows();
+		}
+	}
+}
+
+/**
+  Go through each limit motor and apply the relevant 
+  constraints
+*/
+void dxJointAMPlus::getInfo2( dxJoint::Info2 *info )
+{
+  int row = 0;
+  for (int ii = 0;ii<3;++ii) {
+    row += limot[ii].addRotationalLimot( this, info, cacheAxis[ii], row );
+  }
+}
+
+dJointType dxJointAMPlus::type() const
+{
+    return dJointTypeAMPlus;
+}
+
+
+size_t dxJointAMPlus::size() const
+{
+    return sizeof( *this );
+}
+
+/////////////////////////////////////////////////////////////
+// Public API functions
+/////////////////////////////////////////////////////////////
+void dJointSetAMPlusParam( dJointID j, int parameter, dReal value )
+{
+    dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+    dAASSERT( joint );
+    checktype( joint, AMPlus );
+    int anum = parameter >> 8;
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    parameter &= 0xff;
+    joint->limot[anum].set( parameter, value );
+}
+
+dReal dJointGetAMPlusParam( dJointID j, int parameter )
+{
+    dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+    dAASSERT( joint );
+    checktype( joint, AMPlus );
+    int anum = parameter >> 8;
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    parameter &= 0xff;
+    return joint->limot[anum].get( parameter );
+}
+
+void dJointSetAMPlusAxis1( dJointID j, int anum, dReal x, dReal y, dReal z )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2  );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  setAxes(j,x,y,z,joint->axis1[anum],NULL);
+}
+
+void dJointSetAMPlusAxis1Rel( dJointID j, int anum, dReal x, dReal y, dReal z )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+  
+  joint->axis1[anum][0] = x;
+  joint->axis1[anum][1] = y;
+  joint->axis1[anum][2] = z;
+  dNormalize3(joint->axis1[anum]);
+}
+
+void dJointSetAMPlusAxis1V( dJointID j, int anum, const dVector3 ax)
+{
+  dJointSetAMPlusAxis1(j,anum,ax[0],ax[1],ax[2]);
+}
+
+void dJointSetAMPlusAxis1RelV( dJointID j, int anum, const dVector3 ax)
+{
+  dJointSetAMPlusAxis1Rel(j,anum,ax[0],ax[1],ax[2]);
+}
+
+void dJointSetAMPlusAxis2( dJointID j, int anum, dReal x, dReal y, dReal z )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  setAxes(j,x,y,z,NULL,joint->axis2[anum]);
+}
+
+void dJointSetAMPlusAxis2Rel( dJointID j, int anum, dReal x, dReal y, dReal z )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  joint->axis2[anum][0] = x;
+  joint->axis2[anum][1] = y;
+  joint->axis2[anum][2] = z;
+  dNormalize3(joint->axis2[anum]);
+}
+
+void dJointSetAMPlusAxis2V( dJointID j, int anum, const dVector3 ax)
+{
+  dJointSetAMPlusAxis2(j,anum,ax[0],ax[1],ax[2]);
+}
+void dJointSetAMPlusAxis2RelV( dJointID j, int anum, const dVector3 ax)
+{
+  dJointSetAMPlusAxis2Rel(j,anum,ax[0],ax[1],ax[2]);
+}
+
+void dJointSetAMPlusAxisRef( dJointID j, int anum, int frame, dReal x, dReal y, dReal z )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 && frame >= 1 && frame <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  joint->frame[anum] = frame;
+  if (frame==1) {
+    setAxes(j,x,y,z,joint->axisRef[anum],NULL);
+  } else {
+    setAxes(j,x,y,z,NULL,joint->axisRef[anum]);
+  }
+}
+
+void dJointSetAMPlusAxisRefRel( dJointID j, int anum, int frame, dReal x, dReal y, dReal z )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 && frame >= 1 && frame <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  joint->frame[anum] = frame;
+  joint->axisRef[anum][0] = x;
+  joint->axisRef[anum][1] = y;
+  joint->axisRef[anum][2] = z;
+  dNormalize3(joint->axisRef[anum]);
+}
+
+void dJointSetAMPlusAxisRefV( dJointID j, int anum, int frame, const dVector3 ax)
+{
+  dJointSetAMPlusAxisRef(j,anum,frame,ax[0],ax[1],ax[2]);
+}
+
+void dJointSetAMPlusAxisRefRelV( dJointID j, int anum, int frame, const dVector3 ax)
+{
+  dJointSetAMPlusAxisRefRel(j,anum,frame,ax[0],ax[1],ax[2]);
+}
+
+
+void dJointSetAMPlusMode( dJointID j, int anum, int mode )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  joint->mode[anum]=mode;
+}
+
+/**
+  Turn this joint into an anchorless hinge.
+  We associate the axis with the first body
+  and its planespace with the second body.
+  We constrain those to be orthogonal.
+*/
+void dJointSetAMPlusHingeV( dJointID j, const dVector3 ax )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint );
+  checktype( joint, AMPlus );
+
+  dVector3 pp,qq;
+  //ax[0]=x; ax[1]=y; ax[2]=z;
+  dPlaneSpace(ax,pp,qq);
+
+  setAxes(j,ax[0],ax[1],ax[2],joint->axis1[0],joint->axisRef[2]);
+  //setAxes(j,ax[0],ax[1],ax[2],joint->axisRef[2],0);
+  setAxes(j,ax[0],ax[1],ax[2],joint->axis1[1],0);
+  setAxes(j,pp[0],pp[1],pp[2],joint->axis1[2],joint->axis2[2]);
+  setAxes(j,pp[0],pp[1],pp[2], NULL,joint->axis2[0]);
+  setAxes(j,qq[0],qq[1],qq[2], NULL,joint->axis2[1]);
+
+  joint->mode[0]=joint->mode[1]=dAMPlusCone;
+  
+  joint->mode[2]=dAMPlusProjected;
+  joint->frame[2]=2;
+
+  // Set the orthogonality constraints.
+  dReal aa = M_PI/2;
+  dJointSetAMPlusParam(j,dParamLoStop1,aa);
+  dJointSetAMPlusParam(j,dParamHiStop1,aa);
+  dJointSetAMPlusParam(j,dParamLoStop2,aa);
+  dJointSetAMPlusParam(j,dParamHiStop2,aa);
+}
+
+void dJointSetAMPlusHinge( dJointID j, dReal x, dReal y, dReal z)
+{
+  dVector3 ax;
+  ax[0] = x;
+  ax[1] = y;
+  ax[2] = z;
+
+  dJointSetAMPlusHingeV(j,ax);
+}
+
+/**
+  We constrain ax1 wrt body1 to be orthogonal to ax2 wrt body 2
+  and then provide motors around these 2 axes.
+*/
+void dJointSetAMPlusUniversalV( dJointID j, const dVector3 ax1, const dVector3 ax2)
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint );
+  checktype( joint, AMPlus );
+
+  setAxes(j,ax1[0],ax1[1],ax1[2],joint->axis1[0],joint->axis2[2]);
+  setAxes(j,ax1[0],ax1[1],ax1[2],joint->axisRef[1],0);
+  setAxes(j,ax1[0],ax1[1],ax1[2],joint->axis1[2],0);
+
+  setAxes(j,ax2[0],ax2[1],ax2[2],joint->axis1[1],joint->axis2[0]);
+  setAxes(j,ax2[0],ax2[1],ax2[2],0,joint->axis2[1]);
+  setAxes(j,ax2[0],ax2[1],ax2[2],0,joint->axisRef[2]);
+
+  joint->mode[0] = dAMPlusCone;
+  joint->mode[1] = joint->mode[2] = dAMPlusProjected;
+  joint->frame[1]=1;
+  joint->frame[2]=2;
+
+  // Constrain the axes to be orthogonal
+  dReal aa = M_PI/2;
+  dJointSetAMPlusParam(j,dParamLoStop1,aa);
+  dJointSetAMPlusParam(j,dParamHiStop1,aa);
+}
+
+void dJointSetAMPlusUniversal( dJointID j, dReal x1, dReal y1, dReal z1,
+																			dReal x2, dReal y2, dReal z2)
+{
+  dVector3 ax1,ax2;
+
+  ax1[0]=x1;ax1[1]=y1;ax1[2]=z1;
+  ax2[0]=x2;ax2[1]=y2;ax2[2]=z2;
+
+  dJointSetAMPlusUniversalV(j,ax1,ax2);
+}
+
+void dJointGetAMPlusAxis1( dJointID j, int anum, dVector3 result )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  getAxis( j,result,joint->axis1[anum]);   
+}
+
+void dJointGetAMPlusAxis1Rel( dJointID j, int anum, dVector3 result )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  dCopyVector3(result,joint->axis1[anum]);  
+}
+
+void dJointGetAMPlusAxis2( dJointID j, int anum, dVector3 result )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  getAxis2( j,result,joint->axis2[anum]);   
+}
+
+void dJointGetAMPlusAxis2Rel( dJointID j, int anum, dVector3 result )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  dCopyVector3(result,joint->axis2[anum]);  
+}
+
+void dJointGetAMPlusAxisRef( dJointID j, int anum, dVector3 result )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  if (joint->frame[anum]==1) {
+    getAxis( j,result,joint->axisRef[anum]);   
+  } else {
+    getAxis2( j,result,joint->axisRef[anum]);   
+  }
+}
+
+void dJointGetAMPlusAxisRefRel( dJointID j, int anum, dVector3 result )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  dCopyVector3(result,joint->axisRef[anum]);  
+}
+
+
+int dJointGetAMPlusAxisRefFrame( dJointID j, int anum )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+  
+  return joint->frame[anum];
+}
+
+int dJointGetAMPlusMode( dJointID j, int anum )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+  
+  return joint->mode[anum];
+}
+
+void dJointGetAMPlusEffectiveAxis( const dJointID j, int anum, dVector3 res )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  joint->currentAngle(res,anum);
+}
+
+dReal dJointGetAMPlusAngle( dJointID j, int anum )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  dVector3 ax;
+  return joint->currentAngle(ax,anum);
+}
+
+
+dReal dJointGetAMPlusAngleRate( dJointID j, int anum )
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  dVector3 ax;
+  joint->currentAngle(ax,anum);
+
+  dReal rate=0;
+
+  // Backwards?
+  rate+=dCalcVectorDot3( joint->node[0].body->avel, ax);
+  if (joint->node[1].body) {
+    rate-=dCalcVectorDot3( joint->node[1].body->avel, ax);
+  }
+  return rate;
+}
+
+
+void dJointAddAMPlusTorque( dJointID j, int anum, dReal torque)
+{
+  dxJointAMPlus* joint = ( dxJointAMPlus* )j;
+  dAASSERT( joint && anum >= 0 && anum <= 2 );
+  checktype( joint, AMPlus );
+  if ( anum < 0 ) anum = 0;
+  if ( anum > 2 ) anum = 2;
+
+  dVector3 ax;
+  joint->currentAngle(ax,anum);
+  dScaleVector3(ax,torque);
+
+  dBodyAddTorque( joint->node[0].body, ax[0], ax[1], ax[2] );
+  if (joint->node[1].body) {
+    dBodyAddTorque( joint->node[1].body, -ax[0], -ax[1], -ax[2] );
+  }
+}
+
+
+
Index: ode/src/joints/amplus.h
===================================================================
--- ode/src/joints/amplus.h	(revision 0)
+++ ode/src/joints/amplus.h	(working copy)
@@ -0,0 +1,63 @@
+/*************************************************************************
+ *                                                                       *
+ * AMPlus for Open Dynamics Engine.  Copyright (C) 2012 Joseph Cooper.   *
+ *                                                                       *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *   (3) The zlib/libpng license that is included with this library in   *
+ *       the file LICENSE-ZLIB.TXT                                       *
+ *                                                                       *
+ * This library is distributed WITHOUT ANY WARRANTY, including implied   *
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
+ * See the files LICENSE.TXT or LICENSE-BSD.TXT or LICENSE-ZLIB.TXT      *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_AMPLUS_H_
+#define _ODE_JOINT_AMPLUS_H_
+
+#include "joint.h"
+
+
+// angular motor plus
+
+struct dxJointAMPlus : public dxJoint
+{
+  int mode[3];              // a dAMPlusXXX constant
+  int frame[3];               // what the reference axes are relative to (b1,b2)
+  dVector3 axis1[3];		    // axes wrt the first body
+  dVector3 axis2[3];        // axes wrt the second body
+  // reference axes are the axis around which
+  // axis1 and axis2 spin and around which their 
+  // angles are measured
+  // This should be orthogonal to the associated axis
+  // determined by "rel"
+  // We don't enforce orthogonality so that the behavior
+  // isn't order dependent (you don't have to set one before the other)
+  dVector3 axisRef[3];      
+  dxJointLimitMotor limot[3]; // limit+motor info for axes
+  dVector3 cacheAxis[3];
+
+
+    dxJointAMPlus( dxWorld *w );
+
+  dReal currentAngle(dVector3 axis,int anum);
+  
+  virtual void getSureMaxInfo( SureMaxInfo* info );
+  virtual void getInfo1( Info1* info );
+  virtual void getInfo2( Info2* info );
+  virtual dJointType type() const;
+  virtual size_t size() const;
+};
+
+
+#endif
+
Index: ode/src/joints/hinge.cpp
===================================================================
--- ode/src/joints/hinge.cpp	(revision 1903)
+++ ode/src/joints/hinge.cpp	(working copy)
@@ -47,7 +47,7 @@
 void 
 dxJointHinge::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 5 + limot.countSureMaxRows();
 }
 
 
@@ -55,22 +55,20 @@
 dxJointHinge::getInfo1( dxJoint::Info1 *info )
 {
     info->nub = 5;
-
-    // see if joint is powered
-    if ( limot.fmax > 0 )
-        info->m = 6; // powered hinge needs an extra constraint row
-    else info->m = 5;
-
+    info->m = 5;
+    
     // see if we're at a joint limit.
+    limot.limit=0;
     if (( limot.lostop >= -M_PI || limot.histop <= M_PI ) &&
         limot.lostop <= limot.histop )
     {
         dReal angle = getHingeAngle( node[0].body,
             node[1].body,
             axis1, qrel );
-        if ( limot.testRotationalLimit( angle ) )
-            info->m = 6;
+        limot.testRotationalLimit( angle );
     }
+    info->m += limot.countRows();
+    info->nub += limot.countUBRows();
 }
 
 
@@ -146,7 +144,8 @@
     info->c[4] = k * dCalcVectorDot3( b, q );
 
     // if the hinge is powered, or has joint limits, add in the stuff
-    limot.addLimot( this, info, 5, ax1, 1 );
+    //limot.addLimot( this, info, 5, ax1, 1 );
+    limot.addRotationalLimot( this, info, ax1, 5 );
 }
 
 
Index: ode/src/joints/hinge2.cpp
===================================================================
--- ode/src/joints/hinge2.cpp	(revision 1903)
+++ ode/src/joints/hinge2.cpp	(working copy)
@@ -74,7 +74,7 @@
 void 
 dxJointHinge2::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 4 + limot1.countSureMaxRows() + limot2.countSureMaxRows();
 }
 
 
@@ -92,11 +92,12 @@
         dReal angle = measureAngle();
         limot1.testRotationalLimit( angle );
     }
-    if ( limot1.limit || limot1.fmax > 0 ) info->m++;
+    info->m += limot1.countRows();
+    info->nub+= limot1.countUBRows();
 
     // see if we're powering axis 2 (we currently never limit this axis)
-    limot2.limit = 0;
-    if ( limot2.fmax > 0 ) info->m++;
+    info->m+=limot2.countRows();
+    info->nub+=limot2.countUBRows();
 }
 
 
@@ -171,10 +172,10 @@
     info->c[3] = k * ( c0 * s - joint->s0 * c );
 
     // if the axis1 hinge is powered, or has joint limits, add in more stuff
-    int row = 4 + limot1.addLimot( this, info, 4, ax1, 1 );
+    int row = 4 + limot1.addRotationalLimot( this, info, ax1, 4 );
 
     // if the axis2 hinge is powered, add in more stuff
-    limot2.addLimot( this, info, row, ax2, 1 );
+    limot2.addRotationalLimot( this, info, ax2, row );
 
     // set parameter for the suspension
     info->cfm[0] = susp_cfm;
Index: ode/src/joints/joint.cpp
===================================================================
--- ode/src/joints/joint.cpp	(revision 1903)
+++ ode/src/joints/joint.cpp	(working copy)
@@ -473,7 +473,7 @@
 
 void dxJointLimitMotor::init( dxWorld *world )
 {
-    vel = 0;
+    lo_vel = hi_vel = 0;
     fmax = 0;
     lostop = -dInfinity;
     histop = dInfinity;
@@ -498,13 +498,26 @@
         histop = value;
         break;
     case dParamVel:
-        vel = value;
+        lo_vel = hi_vel = value;
         break;
+    // If hiVel!=loVel, we'll use two motor rows
+    // One row uses fmax to slow down to hiVel
+    // One row uses fmax to speed up to loVel
+    case dParamLoVel:
+        lo_vel = value;
+        break;
+    case dParamHiVel:
+        hi_vel = value;
+        break;
     case dParamFMax:
         if ( value >= 0 ) fmax = value;
         break;
+    //Setting fudge_factor<0 can be used to disable the
+    // fudge_factor entirely, using, instead, an extra row
+    // when it's needed
     case dParamFudgeFactor:
-        if ( value >= 0 && value <= 1 ) fudge_factor = value;
+        //if ( value >= 0 && value <= 1 ) fudge_factor = value;
+        if ( value <= 1 ) fudge_factor = value;
         break;
     case dParamBounce:
         bounce = value;
@@ -531,7 +544,10 @@
     case dParamHiStop:
         return histop;
     case dParamVel:
-        return vel;
+    case dParamLoVel:
+        return lo_vel;
+    case dParamHiVel:
+        return hi_vel;
     case dParamFMax:
         return fmax;
     case dParamFudgeFactor:
@@ -549,6 +565,14 @@
     }
 }
 
+int dxJointLimitMotor::isActive()
+{
+	return ( (fmax>0) || // It's a motor
+		((lostop<=histop) && // It's a limit
+		 ((lostop!=-dInfinity) ||
+		  (histop!=dInfinity))
+		));
+}
 
 int dxJointLimitMotor::testRotationalLimit( dReal angle )
 {
@@ -633,7 +657,7 @@
             info->cfm[row] = normal_cfm;
             if ( ! limit )
             {
-                info->c[row] = vel;
+                info->c[row] = lo_vel;
                 info->lo[row] = -fmax;
                 info->hi[row] = fmax;
             }
@@ -649,10 +673,10 @@
                 // a fudge factor.
 
                 dReal fm = fmax;
-                if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+                if (( lo_vel > 0 ) || ( lo_vel == 0 && limit == 2 ) ) fm = -fm;
 
                 // if we're powering away from the limit, apply the fudge factor
-                if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+                if (( limit == 1 && lo_vel > 0 ) || ( limit == 2 && lo_vel < 0 ) ) fm *= fudge_factor;
 
                 if ( rotational )
                 {
@@ -751,8 +775,322 @@
     else return 0;
 }
 
+/**
+  This function does the generic limit-motor stuff after
+  the LHS has been set.
+*/
+int dxJointLimitMotor::finishLimot(dxJoint *joint, dxJoint::Info2 *info, int row)
+{
+  int rr=0;
+  if (limit) {
+    // We have an active limit.  Set the right hand side.
+    setLimitRHS(joint,info,row);
+    rr+=1; 
+  }
+  if (fmax>0) {
+    // We have an active motor,
+    // If there's no limit or fudge_factor is disabled,
+    // it gets its own row.
+    if (!limit || fudge_factor<0 || hi_vel!=lo_vel) {
+      if (rr>0) { // We've already added a row, copy the data.
+        copyLHS(info,row,row+rr);
+      }
+      if (hi_vel!=lo_vel) {
+        // The motor velocity targets a range; 
+        // so it gets two rows.
+        setLoMotorRHS(info,row+rr);
+        rr+=1;
+        copyLHS(info,row,row+rr);
+        setHiMotorRHS(info,row+rr);
+        rr+=1; 
+      } else {
+        setMotorRHS(info,row+rr);
+        rr+=1; 
+      }
+    } 
+    if (limit && fudge_factor>=0 && hi_vel==lo_vel) {
+      applyMotorFudgeForce(joint,info,row);
+    }
+  }
+  return rr;
+}
 
+/**
+  Set the angular LHS and then finish up.
+*/
+int dxJointLimitMotor::addRotationalLimot(dxJoint *joint, dxJoint::Info2 *info,
+                           const dVector3 ax, int row )
+{
+  if (fmax<=0 && !limit) return 0;
+  setAngularLHS(joint,info,ax,row);
+  return finishLimot(joint,info,row);
+}
+/**
+  Find the torque decoupling if necessary
+  and then set the LHS and finish up.
+*/
+int dxJointLimitMotor::addLinearLimot(dxJoint *joint, dxJoint::Info2 *info,
+                       const dVector3 ax, int row )
+{
+  // linear limot torque decoupling step:
+  //
+  // if this is a linear limot (e.g. from a slider), we have to be careful
+  // that the linear constraint forces (+/- ax1) applied to the two bodies
+  // do not create a torque couple. in other words, the points that the
+  // constraint force is applied at must lie along the same ax1 axis.
+  // a torque couple will result in powered or limited slider-jointed free
+  // bodies from gaining angular momentum.
+  // the solution used here is to apply the constraint forces at the point
+  // halfway between the body centers. there is no penalty (other than an
+  // extra tiny bit of computation) in doing this adjustment. note that we
+  // only need to do this if the constraint connects two bodies.
+  dVector3 pt1 = {0,0,0};
+  dVector3 pt2 = {0,0,0};
+  if (fmax<=0 && !limit) return 0;
+  if ( joint->node[1].body ) {
+    pt1[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] );
+    pt1[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] );
+    pt1[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] );
+    dCopyNegatedVector3(pt2,pt1);
+  }
+  return addPointLinearLimot(joint,info,pt1,pt2,ax,row);
+}
 
+/**
+  Do the cross products that set the LHS
+  and then finish up.
+*/
+int dxJointLimitMotor::addPointLinearLimot( dxJoint *joint,
+                dxJoint::Info2 *info,
+                        const dVector3 pt1, const dVector3 pt2,
+                        const dVector3 ax, int row )
+{
+  if (fmax<=0 && !limit) return 0;
+  setPointLinearLHS(joint,info,pt1,pt2,ax,row);
+  return finishLimot(joint,info,row);
+}
+
+
+void dxJointLimitMotor::copyLHS(dxJoint::Info2 *info,int fromRow,int toRow)
+{
+  int ssFrom = info->rowskip*fromRow;
+  int ssTo   = info->rowskip*toRow;
+  dCopyVector3(&(info->J1l[ssTo]),&(info->J1l[ssFrom]));
+  dCopyVector3(&(info->J1a[ssTo]),&(info->J1a[ssFrom]));
+  dCopyVector3(&(info->J2l[ssTo]),&(info->J2l[ssFrom]));
+  dCopyVector3(&(info->J2a[ssTo]),&(info->J2a[ssFrom]));
+}
+
+void dxJointLimitMotor::setPointLinearLHS(dxJoint *joint,dxJoint::Info2 *info,
+                        const dVector3 pt1, const dVector3 pt2,
+                        const dVector3 ax, int row )
+{
+    int ss = info->rowskip*row;
+
+    // Set the linear portion
+    dCopyVector3(&(info->J1l[ss]),ax);
+    // Set the angular portion (to move the linear constraint 
+    // away from the center of mass).  
+    dCalcVectorCross3(&(info->J1a[ss]),pt1,ax);
+    // Set the constraints for the second body
+    if ( joint->node[1].body ) {
+        dCopyNegatedVector3(&(info->J2l[ss]), ax);
+        dCalcVectorCross3(&(info->J2a[ss]),pt2,&(info->J2l[ss]));
+    }
+}
+
+void dxJointLimitMotor::setAngularLHS(dxJoint *joint,dxJoint::Info2 *info,
+                        const dVector3 ax, int row )
+{
+  int ss = info->rowskip*row;
+  
+  dCopyVector3(&(info->J1a[ss]),ax);
+  if ( joint->node[1].body ) {
+      dCopyNegatedVector3(&(info->J2a[ss]),ax);
+  }
+}
+
+/**
+  Using the error computed in testRotationalLimit(), we
+  set the correcting velocity for the constraint.
+  If bounce is enabled, we use the LHS values to find
+  the current velocity along this degree of freedom
+  and compute the bounce velocity.
+  We also set the CFM and LCP force limits.
+*/
+void dxJointLimitMotor::setLimitRHS(dxJoint *joint,dxJoint::Info2 *info,int row)
+{
+  int srow = row * info->rowskip;
+
+  dReal k = info->fps * stop_erp;
+  info->c[row] = -k * limit_err;
+  info->cfm[row] = stop_cfm;
+
+  if ( lostop == histop )	{
+      // if limited low and high simultaneously,
+      // this is a 'UB' constraint. 
+      // Bounce is not taken into consideration.
+      info->lo[row] = -dInfinity;
+      info->hi[row] = dInfinity;
+  } else {
+      // We're at one limit or the other, but not both.
+      // So we need to find the direction.
+      if ( limit == 1 ) {
+          // low limit
+          info->lo[row] = 0;
+          info->hi[row] = dInfinity;
+      } else {
+          // high limit
+          info->lo[row] = -dInfinity;
+          info->hi[row] = 0;
+      }
+
+  // deal with bounce
+  	if ( bounce > 0 ) {
+  		// calculate joint velocity
+  		dReal vel = 
+  			dCalcVectorDot3( joint->node[0].body->lvel, &(info->J1l[srow])) +
+  			dCalcVectorDot3( joint->node[0].body->avel, &(info->J1a[srow]));
+  		if (joint->node[1].body) {
+  			vel +=
+  				dCalcVectorDot3( joint->node[1].body->lvel, &(info->J2l[srow])) +
+  				dCalcVectorDot3( joint->node[1].body->avel, &(info->J2a[srow]));
+  		}
+
+  		// only apply bounce if the velocity is incoming, and if the
+  		// resulting c[] exceeds what we already have.
+  		if ( limit == 1 ) {
+  			// low limit
+  			if ( vel < 0 ) {
+  				dReal newc = -bounce * vel;
+  				if ( newc > info->c[row] ) info->c[row] = newc;
+  			}
+  		} else {
+  			// high limit - all those computations are reversed
+  			if ( vel > 0 ) {
+  				dReal newc = -bounce * vel;
+  				if ( newc < info->c[row] ) info->c[row] = newc;
+  			}
+  		}
+  	}
+  }
+}
+
+/**
+	Simple and straight-forward.
+  Use up to fmax force/torque to 
+  speed-up/slow-down the bodies.
+*/
+void dxJointLimitMotor::setMotorRHS(dxJoint::Info2 *info,int row)
+{
+    info->cfm[row] = normal_cfm;
+    info->c[row] =  lo_vel;
+    info->lo[row] = -fmax;
+    info->hi[row] = fmax;
+}
+
+/* Speed it up as needed.  Don't slow it down.*/
+void dxJointLimitMotor::setLoMotorRHS(dxJoint::Info2 *info,int row)
+{
+    info->cfm[row] = normal_cfm;
+    info->c[row] =  lo_vel;
+    info->lo[row] = 0;
+    info->hi[row] = fmax;
+}
+
+/* Slow it down as needed.  Don't speed it up. */
+void dxJointLimitMotor::setHiMotorRHS(dxJoint::Info2 *info,int row)
+{
+    info->cfm[row] = normal_cfm;
+    info->c[row] =  hi_vel;
+    info->lo[row] = -fmax;
+    info->hi[row] = 0;
+}
+
+/**
+  If we're applying this function, we assume that lo_vel==hi_vel
+  and that a limit constraint has already been set in the 
+  specified row.
+*/
+void dxJointLimitMotor::applyMotorFudgeForce(dxJoint *joint,dxJoint::Info2 *info,int row)
+{
+  int srow = row * info->rowskip;
+  dReal fm = (( lo_vel > 0 ) || ( lo_vel == 0 && limit == 2 ) )?-fmax:fmax;
+  // if we're powering away from the limit, apply the fudge factor
+  if (( limit == 1 && lo_vel > 0 ) || ( limit == 2 && lo_vel < 0 ) ) fm *= fudge_factor;
+  
+  dReal* lf = &(info->J1l[srow]);
+  dReal* af = &(info->J1a[srow]);
+
+  dBodyAddForce(joint->node[0].body,lf[0]*fm,lf[1]*fm,lf[2]*fm);
+  dBodyAddTorque(joint->node[0].body,af[0]*fm,af[1]*fm,af[2]*fm);
+  if (joint->node[1].body) {
+    lf = &(info->J2l[srow]);
+    af = &(info->J2a[srow]);
+    dBodyAddForce(joint->node[1].body,lf[0]*fm,lf[1]*fm,lf[2]*fm);
+    dBodyAddTorque(joint->node[1].body,af[0]*fm,af[1]*fm,af[2]*fm);
+  }
+}
+
+/** 
+  We can quickly determine about how many rows we'll
+  use.  We're not sure about the the limits.
+  This information might change comparatively rarely.
+  Perhaps it should be cached.
+*/
+int dxJointLimitMotor::countSureMaxRows()
+{
+  int rr=0;
+
+  if (fmax>0) { // The motor is active
+    if (lo_vel!=hi_vel) { // We are using a velocity range.
+      if (lostop!=-dInfinity || histop!=dInfinity) { // There might be an active limit
+        rr = 3; // 2 motor rows and a limit row
+      } else {
+        rr = 2; // 2 motor rows
+      }
+    } else if (fudge_factor<0 &&
+      (lostop!=-dInfinity || histop!=dInfinity)) 
+    {
+      rr = 2; // 1 motor row, 1 limit row
+    } else {
+      rr = 1; // 1 motor/limit row (possible fudge factor use)
+    }
+  } else if (lostop!=-dInfinity || histop!=dInfinity) {
+    rr = 1; // 1 limit row but no motors
+  }
+  return rr;
+}
+
+int dxJointLimitMotor::countRows()
+{
+  int rr=0;
+  if (fmax>0) { // The motor is active
+    if (lo_vel!=hi_vel) { // We are using a velocity range.
+      if (limit) { // There is an active limit
+        rr = 3; // 2 motor rows and a limit row
+      } else {
+        rr = 2; // 2 motor rows
+      }
+    } else if (fudge_factor<0 && limit) {
+      rr = 2; // 1 motor row, 1 limit row
+    } else {
+      rr = 1; // 1 motor/limit row (possible fudge factor use)
+    }
+  } else if (limit) {
+    rr = 1; // 1 limit row but no motors
+  }
+  return rr;
+}
+
+int dxJointLimitMotor::countUBRows()
+{
+    int rr=0;
+    if (lostop==histop) rr+=1;
+    if (fmax==dInfinity && lo_vel==hi_vel) rr+=1;
+    return rr;
+}
+
 // Local Variables:
 // mode:c++
 // c-basic-offset:4
Index: ode/src/joints/joint.h
===================================================================
--- ode/src/joints/joint.h	(revision 1903)
+++ ode/src/joints/joint.h	(working copy)
@@ -193,7 +193,7 @@
 // common limit and motor information for a single joint axis of movement
 struct dxJointLimitMotor
 {
-    dReal vel, fmax;        // powered joint: velocity, max force
+    dReal lo_vel, hi_vel, fmax;  // powered joint: velocity, max force
     dReal lostop, histop;   // joint limits, relative to initial position
     dReal fudge_factor;     // when powering away from joint limits
     dReal normal_cfm;       // cfm to use when not at a stop
@@ -206,9 +206,43 @@
     void init( dxWorld * );
     void set( int num, dReal value );
     dReal get( int num );
+
+    int isActive();
+
     int testRotationalLimit( dReal angle );
     int addLimot( dxJoint *joint, dxJoint::Info2 *info, int row,
         const dVector3 ax1, int rotational );
+    int finishLimot(dxJoint *joint, dxJoint::Info2 *info, int row);
+
+    int addRotationalLimot(dxJoint *joint, dxJoint::Info2 *info,
+                           const dVector3 ax, int row );
+    int addLinearLimot(dxJoint *joint, dxJoint::Info2 *info,
+                       const dVector3 ax, int row );
+    int addPointLinearLimot( dxJoint *joint,
+                        dxJoint::Info2 *info,
+                        const dVector3 pt1, const dVector3 pt2,
+                        const dVector3 ax, int row );
+    void copyLHS(dxJoint::Info2 *info,int fromRow,int toRow);
+    void setLinearLHS(dxJoint *joint,dxJoint::Info2 *info,
+                        const dVector3 ax, int row );
+    void setPointLinearLHS(dxJoint *joint,dxJoint::Info2 *info,
+         const dVector3 pt1, const dVector3 pt2,
+         const dVector3 ax, int row );
+    void setAngularLHS(dxJoint *joint,dxJoint::Info2 *info,
+                        const dVector3 ax, int row );
+    /// This function assumes that the LHS is already set!
+    /// It uses that to compute the bounce velocity if needed.
+    void setLimitRHS(dxJoint *joint,dxJoint::Info2 *info,int row);
+    void setMotorRHS(dxJoint::Info2 *info,int row);
+    void setLoMotorRHS(dxJoint::Info2 *info,int row);
+    void setHiMotorRHS(dxJoint::Info2 *info,int row);
+    /// This also assumes that the LHS is already set
+    /// and applies the force along the jacobian
+    void applyMotorFudgeForce(dxJoint *joint,dxJoint::Info2 *info,int row);
+
+    int countSureMaxRows();
+    int countRows();
+    int countUBRows();
 };
 
 
Index: ode/src/joints/joints.h
===================================================================
--- ode/src/joints/joints.h	(revision 1903)
+++ ode/src/joints/joints.h	(working copy)
@@ -43,5 +43,7 @@
 #include "pu.h"
 #include "pr.h"
 #include "piston.h"
+#include "lmplus.h"
+#include "amplus.h"
 
 #endif
Index: ode/src/joints/lmotor.cpp
===================================================================
--- ode/src/joints/lmotor.cpp	(revision 1903)
+++ ode/src/joints/lmotor.cpp	(working copy)
@@ -69,7 +69,11 @@
 void 
 dxJointLMotor::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = num;
+    info->max_m = 0;
+    for ( int i = 0; i < num; i++ )
+    {
+      info->max_m += limot[i].countSureMaxRows();
+    }
 }
 
 void
@@ -79,10 +83,8 @@
     info->nub = 0;
     for ( int i = 0; i < num; i++ )
     {
-        if ( limot[i].fmax > 0 )
-        {
-            info->m++;
-        }
+      info->m+=limot[i].countRows();
+      info->nub+=limot[i].countUBRows();
     }
 }
 
@@ -95,7 +97,7 @@
 
     for ( int i = 0;i < num;i++ )
     {
-        row += limot[i].addLimot( this, info, row, ax[i], 0 );
+        row += limot[i].addLinearLimot( this, info,  ax[i], row);
     }
 }
 
Index: ode/src/joints/lmplus.cpp
===================================================================
--- ode/src/joints/lmplus.cpp	(revision 0)
+++ ode/src/joints/lmplus.cpp	(working copy)
@@ -0,0 +1,672 @@
+/*************************************************************************
+ *                                                                       *
+ * LMPlus for Open Dynamics Engine.  Copyright (C) 2012 Joseph Cooper.   *
+ *                                                                       *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *   (3) The zlib/libpng license that is included with this library in   *
+ *       the file LICENSE-ZLIB.TXT                                       *
+ *                                                                       *
+ * This library is distributed WITHOUT ANY WARRANTY, including implied   *
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
+ * See the files LICENSE.TXT or LICENSE-BSD.TXT or LICENSE-ZLIB.TXT      *
+ *                                                                       *
+ *************************************************************************/
+
+
+#include "config.h"
+#include "lmplus.h"
+#include "joint_internal.h"
+
+
+//****************************************************************************
+// lmplus joint
+dxJointLMPlus::dxJointLMPlus( dxWorld *w ) 
+: dxJoint( w )
+{
+    int ii;
+    for ( ii = 0;ii < 3;ii++ ) {
+			frame[ii]=0; // Default to world frame
+      dSetZero( axis[ii], 4 );
+			axis[ii][ii]=1; // Default to x,y,z axes
+      limot[ii].init( world ); // Initialize the limots
+			mode[ii] = 0; // Linear (box) mode.
+    }
+		dSetZero(anchor1,4);  // Default anchor position to CoM of body
+		dSetZero(anchor2,4);
+}
+	
+/**
+	In order to create a constraint row,
+	we need to know the global axis, the global anchors,
+	the current distance between the anchors
+	along the global axis, and the desired value.
+	For unilateral limits, we also need to know the relative velocity
+	of the points along the axis.
+*/
+dReal dxJointLMPlus::interPointDistance(dVector3 ax,int ii)
+{
+	dVector3 pt1, pt2;
+	dReal dist;
+
+	dVector3 sepVec;
+
+	// Move the anchors from local frames
+	// into the world frame.
+	getAnchor(this, pt1, anchor1);
+	getAnchor2(this, pt2, anchor2);
+
+	// Find the vector between anchor1 and anchor2
+	dSubtractVectors3(sepVec,pt1,pt2);
+
+	switch(mode[ii]) {
+		case dLMPlusPlanar: // The planar case 
+		default:
+			// Rotate the axis from its local frame of reference
+			// into the world frame
+			computeGlobalAxis(ax,ii);
+			dist = dCalcVectorDot3(ax,sepVec);
+			break;
+		case dLMPlusCylindrical: {// The cylindrical case
+			dVector3 norm,qq;
+			computeGlobalAxis(norm,ii);
+			
+			// Find the anchor separation vector
+			// as projected onto the planeSpace
+			if (dDirectedPlaneSpace(norm,sepVec,ax,qq)) {
+				dist = dCalcVectorDot3(ax,sepVec);
+			} else {
+				// The projected anchors are on top of each other
+				dist = 0;
+				// We'll try to find an axis based on the relative
+				// velocity of the anchors since they're moving that 
+				// way already.  Better still might be to use the 
+				// previous frame's axis; but doing so would introduce
+				// some nastiness.
+				dVector3 av1,av2;
+				if (node[0].body) {
+					dBodyGetRelPointVel(node[0].body,anchor1[0],anchor1[1],anchor1[2],av1);
+				} else {
+					dSetZero(av1,3);
+				}
+				if (node[1].body) {
+					dBodyGetRelPointVel(node[1].body,anchor2[0],anchor2[1],anchor2[2],av2);
+				} else {
+					dSetZero(av2,3);
+				}
+				// Find the relative velocity of the anchor points
+				dVector3 relVel;
+				dSubtractVectors3(relVel,av2,av1);
+				// Project the velocity onto the planespace.
+				dReal rvP = dCalcVectorDot3(ax,relVel);
+				dReal rvQ = dCalcVectorDot3(qq,relVel);
+				if (rvP!=0 || rvQ!=0) {
+					dReal lenRecip = dRecipSqrt(rvP*rvP + rvQ*rvQ);
+					dAddScaledVectors3(ax,ax,qq,rvP*lenRecip,rvQ*lenRecip);
+				} else {
+					// The projected relative velocity is also zero.
+					// So we just use the arbitrary axis found by dPlaneSpace.
+				}
+			}
+		} break;
+		case dLMPlusSpherical: { // The spherical case
+			if (sepVec[0]!=0 || sepVec[1]!=0 || sepVec[2]!=0) {
+				// The vector between the two anchors is the axis
+				// of control.
+				dist = dSqrt(sepVec[0]*sepVec[0] + sepVec[1]*sepVec[1] + sepVec[2]*sepVec[2]);
+				dCopyScaledVector3(ax,sepVec,dRecip(dist));
+			} else {
+				// As in the cylindrical case, if the anchors are
+				// on top of each other, we'll try using the relative 
+				// velocity.
+				dist = 0;
+
+				dVector3 av1,av2;
+				if (node[0].body) {
+					dBodyGetRelPointVel(node[0].body,anchor1[0],anchor1[1],anchor1[2],av1);
+				} else {
+					dSetZero(av1,3);
+				}
+				if (node[1].body) {
+					dBodyGetRelPointVel(node[1].body,anchor2[0],anchor2[1],anchor2[2],av2);
+				} else {
+					dSetZero(av2,3);
+				}
+				// Find the relative velocity of the anchor points
+				
+				dSubtractVectors3(ax,av2,av1);
+				if (ax[0]!=0 || ax[1]!=0 || ax[2]!=0) {
+					dNormalize3(ax);				
+				} else {
+					// Use a default axis
+					// Our other option would be 
+					// to leave ax[] unchanged.  Since we 
+					// cache the value, this could be a good
+					// choice, but doing so adds unexposed 
+					// state that could make a joint behave
+					// differently in different trials
+					// even when set with the same values.
+					ax[0] = 1;
+					ax[1] = 0;
+					ax[2] = 0;
+				}
+			}
+		} break;
+	}
+	return dist;
+}
+
+/**
+	Find the axis that constrains the 
+	anchor points.
+
+	We naturally find the distance between
+	the points in the process.
+*/
+void
+dxJointLMPlus::computeGlobalAxis( dVector3 ax, int ii )
+{
+	
+	// If we're in linear or planar mode, we need
+	// to find the axis in global coordinates
+	if (frame[ii]==1 && node[0].body) {
+		dMultiply0_331( ax, node[0].body->posr.R, axis[ii] );
+	} else if (frame[ii]==2 && node[1].body) {
+		dMultiply0_331( ax, node[1].body->posr.R, axis[ii] );
+	} else {
+		dCopyVector3(ax,axis[ii]);
+	}
+}
+
+
+void 
+dxJointLMPlus::getSureMaxInfo( SureMaxInfo* info )
+{
+  info->max_m = 0;
+  for (int ii=0;ii<3;++ii) {
+    info->max_m += limot[ii].countSureMaxRows();
+  }
+}
+
+/**
+	To determine how many constraints are active,
+	we'll check to see if hiStop/loStop are set
+	or if fMax is non-zero.  If hiStop/loStop are set
+	and they're equal, we have a constraint and it
+	doesn't matter what fMax says.  
+	If they're past each other, the constraint is disabled.
+	Otherwise,
+	we need to check to see if we're past/at one of the 
+	stops.  That requires finding that axes and points in 
+	world	coordinates.  It's annoying that we'll have to
+	do that again for getInfo2.
+*/
+void dxJointLMPlus::getInfo1( dxJoint::Info1 *info )
+{
+  info->m = 0;
+  info->nub = 0;
+
+	// For each limot'ed axis
+	for (int ii=0;ii<3;++ii) {
+		if (limot[ii].isActive()) {
+			// Compute the axis/distance and cache them.
+			dReal dist = interPointDistance(cacheAxis[ii],ii);
+			// This mis-named function sets the error
+			// and direction of the error in the limit-motor
+			limot[ii].testRotationalLimit( dist );
+
+			info->m += limot[ii].countRows();
+			info->nub += limot[ii].countUBRows();
+		}
+	}
+}
+
+/**
+	For each degree of freedom, set the constraint rows.
+*/
+void dxJointLMPlus::getInfo2( dxJoint::Info2 *info )
+{
+	int row = 0;
+	dVector3 pt1, pt2;
+
+	// We need the anchors re-oriented to the
+	// the global frame, but not translated.
+	// getAxis() doesn't add in the body's POR
+	// like getAnchor() does; so we use it.
+	getAxis (this,pt1, anchor1);
+	getAxis2(this,pt2, anchor2);
+
+	for (int ii=0;ii<3;++ii) {
+		row += limot[ii].addPointLinearLimot(this,info,pt1,pt2,cacheAxis[ii],row);
+	}
+}
+
+dJointType dxJointLMPlus::type() const
+{
+  return dJointTypeLMPlus;
+}
+
+
+size_t dxJointLMPlus::size() const
+{
+  return sizeof( *this );
+}
+
+/////////////////////////////////////////////////////////////
+// Public API functions
+/////////////////////////////////////////////////////////////
+void dJointSetLMPlusAnchors( dJointID j, dReal x1, dReal y1, dReal z1, dReal x2, dReal y2, dReal z2)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( joint );
+	checktype( joint, LMPlus );
+
+	setAnchors(joint, x1, y1, z1, joint->anchor1, 0);
+	setAnchors(joint, x2, y2, z2, 0, joint->anchor2);
+}
+
+void dJointSetLMPlusAnchorsV( dJointID j, const dVector3 a1, const dVector3 a2)
+{
+	dAASSERT( 		
+		a1 &&
+		a2
+	);
+
+	dJointSetLMPlusAnchors( j, a1[0], a1[1], a1[2], a2[0], a2[1], a2[2]);
+}
+
+void dJointSetLMPlusAnchorsRel( dJointID j, dReal x1, dReal y1, dReal z1, dReal x2, dReal y2, dReal z2)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( joint );
+	checktype( joint, LMPlus );
+
+	joint->anchor1[0]=x1;
+	joint->anchor1[1]=y1;
+	joint->anchor1[2]=z1;
+
+	joint->anchor2[0]=x2;
+	joint->anchor2[1]=y2;
+	joint->anchor2[2]=z2;
+}
+
+void dJointSetLMPlusAnchorsRelV( dJointID j, const dVector3 a1, const dVector3 a2)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		a1 &&
+		a2
+	);
+	checktype( joint, LMPlus );
+
+	dCopyVector3(joint->anchor1, a1);
+	dCopyVector3(joint->anchor2, a2);
+}
+
+void dJointSetLMPlusAnchor1( dJointID j, dReal x1, dReal y1, dReal z1)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( joint );
+	checktype( joint, LMPlus );
+
+	setAnchors(joint, x1, y1, z1, joint->anchor1, 0);
+}
+
+void dJointSetLMPlusAnchor1V( dJointID j, const dVector3 a1)
+{
+	dAASSERT( a1 );
+
+	dJointSetLMPlusAnchor1(j, a1[0], a1[1], a1[2]);
+}
+
+void dJointSetLMPlusAnchor2( dJointID j, dReal x2, dReal y2, dReal z2)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( joint );
+	checktype( joint, LMPlus );
+
+	setAnchors(joint, x2, y2, z2, 0, joint->anchor2);
+}
+
+void dJointSetLMPlusAnchor2V( dJointID j, const dVector3 a2)
+{
+	dAASSERT(	a2 );
+
+	dJointSetLMPlusAnchor2(j, a2[0], a2[1], a2[2] );
+}
+
+void dJointSetLMPlusAnchor1Rel( dJointID j, dReal x1, dReal y1, dReal z1)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( joint );
+	checktype( joint, LMPlus );
+
+	joint->anchor1[0] = x1;
+	joint->anchor1[1] = y1;
+	joint->anchor1[2] = z1;
+}
+
+void dJointSetLMPlusAnchor1RelV( dJointID j, const dVector3 a1)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		a1
+	);
+	checktype( joint, LMPlus );
+
+	dCopyVector3(joint->anchor1,a1);
+}
+
+void dJointSetLMPlusAnchor2Rel( dJointID j, dReal x2, dReal y2, dReal z2)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( joint );
+	checktype( joint, LMPlus );
+
+	joint->anchor2[0] = x2;
+	joint->anchor2[1] = y2;
+	joint->anchor2[2] = z2;
+}
+
+
+void dJointSetLMPlusAnchor2RelV( dJointID j, const dVector3 a2)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		a2
+	);
+	checktype( joint, LMPlus );
+
+	dCopyVector3(joint->anchor2,a2);
+}
+
+void dJointSetLMPlusAxis( dJointID j, int anum, int frame, dReal x, dReal y, dReal z)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2 &&
+		frame >= 0 &&
+		frame <= 2
+	);
+	checktype( joint, LMPlus );
+
+	joint->frame[anum] = frame;
+
+	// Transform the axis from global frame
+	// into 'frame' frame.
+	switch (frame) {
+		case 1:
+			setAxes(joint, x,y,z, joint->axis[anum], 0);
+			break;
+		case 2:
+			setAxes(joint, x,y,z, 0, joint->axis[anum]);
+			break;
+		case 0:
+		default:
+			joint->axis[anum][0] = x;
+			joint->axis[anum][1] = y;
+			joint->axis[anum][2] = z;
+			break;
+	}
+}
+
+void dJointSetLMPlusAxisV( dJointID j, int anum, int frame, const dVector3 ax)
+{
+	dAASSERT( ax );
+	dJointSetLMPlusAxis(j, anum, frame, ax[0], ax[1], ax[2]);
+}
+
+void dJointSetLMPlusAxisRel( dJointID j, int anum, int frame, dReal x, dReal y, dReal z)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2 &&
+		frame >= 0 &&
+		frame <= 2
+	);
+	checktype( joint, LMPlus );
+
+	joint->frame[anum] = frame;
+	joint->axis[anum][0] = x;
+	joint->axis[anum][1] = y;
+	joint->axis[anum][2] = z;
+}
+
+void dJointSetLMPlusAxisRelV( dJointID j, int anum, int frame, const dVector3 ax)
+{
+	dAASSERT( ax );
+
+	dJointSetLMPlusAxisRel(j,anum,frame,ax[0],ax[1],ax[2]);
+}
+
+void dJointSetLMPlusMode( dJointID j, int anum, int mode)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2
+	);
+	checktype( joint, LMPlus );
+
+	joint->mode[anum] = mode;
+}
+
+void dJointGetLMPlusAnchor1( const dJointID j, dVector3 res)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		res
+	);
+	checktype( joint, LMPlus );
+
+	getAnchor(joint, res, joint->anchor1);
+}
+
+void dJointGetLMPlusAnchor1Rel( const dJointID j, dVector3 res)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		res
+	);
+	checktype( joint, LMPlus );
+
+	dCopyVector3(res, joint->anchor1);
+}
+
+void dJointGetLMPlusAnchor2( const dJointID j, dVector3 res)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		res
+	);
+	checktype( joint, LMPlus );
+
+	getAnchor2(joint, res, joint->anchor2);
+}
+
+void dJointGetLMPlusAnchor2Rel( const dJointID j, dVector3 res)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		res
+	);
+	checktype( joint, LMPlus );
+
+	dCopyVector3(res, joint->anchor2);
+}
+
+void dJointGetLMPlusAxis( const dJointID j, int anum, dVector3 res)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2 &&
+		res
+	);
+	checktype( joint, LMPlus );
+
+	switch (joint->frame[anum]) {
+		case 1:
+			getAxis(joint, res, joint->axis[anum]);
+			break;
+		case 2:
+			getAxis2(joint, res, joint->axis[anum]);
+			break;
+		case 0:
+		default:
+			dCopyVector3(res, joint->axis[anum]);
+			break;
+	}
+}
+
+void dJointGetLMPlusAxisRel( const dJointID j, int anum, dVector3 res)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2 &&
+		res
+	);
+	checktype( joint, LMPlus );
+
+	dCopyVector3(res, joint->axis[anum]);
+}
+
+int dJointGetLMPlusAxisFrame( const dJointID j, int anum)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2
+	);
+	checktype( joint, LMPlus );
+
+	return joint->frame[anum];
+}
+
+int dJointGetLMPlusMode( const dJointID j, int anum)
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2
+	);
+	checktype( joint, LMPlus );
+
+	return joint->mode[anum];
+}
+
+// In planar or spherical mode,
+// the actual axis of constraint is
+// not given in axis[n]
+void dJointGetLMPlusEffectiveAxis( const dJointID j, int anum, dVector3 res )
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2
+	);
+	checktype( joint, LMPlus );
+
+	joint->interPointDistance(res, anum);
+	
+}
+
+// How far apart are the anchors along the effective axis
+dReal dJointGetLMPlusSeparation( const dJointID j, int anum )
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2
+	);
+	checktype( joint, LMPlus );
+
+	dVector3 ax;
+	return joint->interPointDistance(ax,anum);
+}
+
+// How fast are the anchors moving along the effective axis
+dReal dJointGetLMPlusSeparationRate( const dJointID j, int anum )
+{
+	dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+	dAASSERT( 
+		joint && 
+		anum >= 0 && 
+		anum <= 2
+	);
+	checktype( joint, LMPlus );
+
+	dVector3 ax,ang;
+	dVector3 pt;
+
+	joint->interPointDistance(ax,anum);
+
+	getAxis (joint,pt,joint->anchor1);
+	dCalcVectorCross3(ang,pt,ax);
+
+	dReal vel = dCalcVectorDot3( joint->node[0].body->lvel, ax) +
+				      dCalcVectorDot3( joint->node[0].body->avel, ang);
+	if (joint->node[1].body) {
+		getAxis2(joint,pt,joint->anchor2);
+		dCalcVectorCross3(ang,pt,ax);
+		vel -=
+					dCalcVectorDot3( joint->node[1].body->lvel, ax) +
+					dCalcVectorDot3( joint->node[1].body->avel, ang);
+	}
+
+	return vel;
+}
+
+void dJointSetLMPlusParam( dJointID j, int parameter, dReal value )
+{
+    dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+    dAASSERT( joint );
+    checktype( joint, LMPlus );
+    int anum = parameter >> 8;
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    parameter &= 0xff;
+    joint->limot[anum].set( parameter, value );
+}
+
+dReal dJointGetLMPlusParam( dJointID j, int parameter )
+{
+    dxJointLMPlus* joint = ( dxJointLMPlus* )j;
+    dAASSERT( joint );
+    checktype( joint, LMPlus );
+    int anum = parameter >> 8;
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    parameter &= 0xff;
+    return joint->limot[anum].get( parameter );
+}
+
+
+
Index: ode/src/joints/lmplus.h
===================================================================
--- ode/src/joints/lmplus.h	(revision 0)
+++ ode/src/joints/lmplus.h	(working copy)
@@ -0,0 +1,84 @@
+/*************************************************************************
+ *                                                                       *
+ * LMPlus for Open Dynamics Engine.  Copyright (C) 2012 Joseph Cooper.   *
+ *                                                                       *
+ *                                                                       *
+ * 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 LICENSE-BSD.TXT.                                       *
+ *   (3) The zlib/libpng license that is included with this library in   *
+ *       the file LICENSE-ZLIB.TXT                                       *
+ *                                                                       *
+ * This library is distributed WITHOUT ANY WARRANTY, including implied   *
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.      *
+ * See the files LICENSE.TXT or LICENSE-BSD.TXT or LICENSE-ZLIB.TXT      *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_LMPLUS_H_
+#define _ODE_JOINT_LMPLUS_H_
+
+#include "joint.h"
+
+/*
+	The LMotor controls the linear velocity between
+	the center of mass of two bodies or a body and the world.
+	This joint builds on that, allowing you to set an 
+	arbitrary anchor point besides the center of mass for
+	each body.  It also lets you constrain the distance 
+	between those anchors instead of just the velocity.
+*/
+struct dxJointLMPlus : public dxJoint
+{
+    int frame[3];       // frame of reference for axis[]
+		dVector3 anchor1; // anchor w.r.t. body1
+		dVector3 anchor2; // anchor w.r.t. body2
+    dVector3 axis[3]; // axis along which the constraint is applied
+		// dLMPlusLinear, dLMPlusPlanar, dLMPlusSpherical
+		// In linear mode, the anchor points are constrained
+		// along the axis[].  A single axis in linear mode
+		// will constrain one anchor to a plane relative to
+		// the other anchor.
+		// In planar mode, the anchor points are constrained
+		// within the planespace defined by the axis[].  A single
+		// axis in planar mode will constrain one anchor to
+		// a cylinder relative to the other anchor
+		// In spherical mode, the axis[] isn't used.  The anchors
+		// are constrained to maintain offset[] distance from 
+		// each other.  A single axis in spherical mode will 
+		// constrain one anchor to a sphere relative to the other 
+		// anchor.
+		
+		int mode[3];  
+		// The limot holds hiStop/loStop information
+		// used by the joint.
+		// A negative loStop/hiStop doesn't make sense for
+		// the planar/spherical modes because they use 
+		// absolute distance.
+    dxJointLimitMotor limot[3];
+		
+		// We need to compute the effective axis in "getInfo1".
+		// Since the computation is non-trivial, we cache
+		// it here.
+		dVector3 cacheAxis[3];
+
+    void computeGlobalAxis( dVector3 ax, int ii );
+		dReal interPointDistance(dVector3 ax, int ii);
+
+    dxJointLMPlus( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( Info2* info );
+    virtual dJointType type() const;
+    virtual size_t size() const;
+};
+
+
+#endif
+
Index: ode/src/joints/Makefile.am
===================================================================
--- ode/src/joints/Makefile.am	(revision 1903)
+++ ode/src/joints/Makefile.am	(working copy)
@@ -22,5 +22,7 @@
                         plane2d.h plane2d.cpp \
                         pu.h pu.cpp \
                         pr.h pr.cpp \
-                        piston.h piston.cpp
+                        piston.h piston.cpp \
+                        amplus.h amplus.cpp \
+                        lmplus.h lmplus.cpp
 
Index: ode/src/joints/piston.cpp
===================================================================
--- ode/src/joints/piston.cpp	(revision 1903)
+++ ode/src/joints/piston.cpp	(working copy)
@@ -175,7 +175,7 @@
 void 
 dxJointPiston::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 4 + limotP.countSureMaxRows() + limotR.countSureMaxRows();
 }
 
 
@@ -198,7 +198,8 @@
     }
 
     // powered Piston or at limits needs an extra constraint row
-    if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+    info->m += limotP.countRows();
+    info->nub += limotP.countUBRows();
 
 
     // see if we're at a joint limit.
@@ -213,7 +214,8 @@
     }
 
     // powered Piston or at limits needs an extra constraint row
-    if ( limotR.limit || limotR.fmax > 0 ) info->m++;
+    info->m += limotR.countRows();
+    info->nub += limotR.countUBRows();
 
 }
 
@@ -406,7 +408,7 @@
     int row = 4;
     if (  node[1].body )
     {
-        row += limotP.addLimot ( this, info, 4, ax1, 0 );
+        row += limotP.addLinearLimot ( this, info,  ax1, 4 );
     }
     else if (flags & dJOINT_REVERSE )
     {
@@ -414,12 +416,12 @@
         rAx1[0] = -ax1[0];
         rAx1[1] = -ax1[1];
         rAx1[2] = -ax1[2];
-        row += limotP.addLimot ( this, info, 4, rAx1, 0 );
+        row += limotP.addLinearLimot ( this, info,  rAx1, 4 );
     }
     else
-        row += limotP.addLimot ( this, info, 4, ax1, 0 );
+        row += limotP.addLinearLimot ( this, info, ax1, 4 );
 
-    limotR.addLimot ( this, info, row, ax1, 1 );
+    limotR.addRotationalLimot ( this, info, ax1, row );
 }
 
 void dJointSetPistonAnchor ( dJointID j, dReal x, dReal y, dReal z )
Index: ode/src/joints/plane2d.cpp
===================================================================
--- ode/src/joints/plane2d.cpp	(revision 1903)
+++ ode/src/joints/plane2d.cpp	(working copy)
@@ -64,7 +64,10 @@
 void 
 dxJointPlane2D::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+    info->max_m = 3 +
+      motor_x.countSureMaxRows() +
+      motor_y.countSureMaxRows() +
+      motor_angle.countSureMaxRows();
 }
 
 
@@ -74,20 +77,12 @@
     info->nub = 3;
     info->m = 3;
 
-    if ( motor_x.fmax > 0 )
-        row_motor_x = info->m++;
-    else
-        row_motor_x = 0;
-
-    if ( motor_y.fmax > 0 )
-        row_motor_y = info->m++;
-    else
-        row_motor_y = 0;
-
-    if ( motor_angle.fmax > 0 )
-        row_motor_angle = info->m++;
-    else
-        row_motor_angle = 0;
+    info->m += motor_x.countRows();
+    info->nub += motor_x.countUBRows();
+    info->m += motor_y.countRows();
+    info->nub += motor_y.countUBRows();
+    info->m += motor_angle.countRows();
+    info->nub += motor_angle.countUBRows();
 }
 
 
@@ -139,14 +134,10 @@
 
     // if the slider is powered, or has joint limits, add in the extra row:
 
-    if ( row_motor_x > 0 )
-        motor_x.addLimot( this, info, row_motor_x, Midentity[0], 0 );
-
-    if ( row_motor_y > 0 )
-        motor_y.addLimot( this, info, row_motor_y, Midentity[1], 0 );
-
-    if ( row_motor_angle > 0 )
-        motor_angle.addLimot( this, info, row_motor_angle, Midentity[2], 1 );
+    int row = 3;
+    row += motor_x.addLinearLimot( this, info,  Midentity[0], row );
+    row += motor_y.addLinearLimot( this, info,  Midentity[1], row );
+    motor_angle.addRotationalLimot( this, info, Midentity[2], row );
 }
 
 
Index: ode/src/joints/plane2d.h
===================================================================
--- ode/src/joints/plane2d.h	(revision 1903)
+++ ode/src/joints/plane2d.h	(working copy)
@@ -30,9 +30,9 @@
 
 struct dxJointPlane2D : public dxJoint
 {
-    int                 row_motor_x;
-    int                 row_motor_y;
-    int                 row_motor_angle;
+    //int                 row_motor_x;
+    //int                 row_motor_y;
+    //int                 row_motor_angle;
     dxJointLimitMotor   motor_x;
     dxJointLimitMotor   motor_y;
     dxJointLimitMotor   motor_angle;
Index: ode/src/joints/pr.cpp
===================================================================
--- ode/src/joints/pr.cpp	(revision 1903)
+++ ode/src/joints/pr.cpp	(working copy)
@@ -180,7 +180,9 @@
 void 
 dxJointPR::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 4 + 
+    limotR.countSureMaxRows() +
+    limotP.countSureMaxRows();
 }
 
 
@@ -203,7 +205,8 @@
     }
 
     // powered needs an extra constraint row
-    if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+    info->m += limotP.countRows();
+    info->nub += limotP.countUBRows();
 
 
     // see if we're at a joint limit.
@@ -430,7 +433,7 @@
     int row = 4;
     if (  node[1].body || !(flags & dJOINT_REVERSE) )
     {
-        row += limotP.addLimot ( this, info, 4, axP, 0 );
+        row += limotP.addLinearLimot ( this, info, axP, 4 );
     }
     else
     {
@@ -438,10 +441,10 @@
         rAxP[0] = -axP[0];
         rAxP[1] = -axP[1];
         rAxP[2] = -axP[2];
-        row += limotP.addLimot ( this, info, 4, rAxP, 0 );
+        row += limotP.addLinearLimot ( this, info, rAxP, 4 );
     }
 
-    limotR.addLimot ( this, info, row, ax1, 1 );
+    limotR.addRotationalLimot ( this, info, ax1, row );
 }
 
 
Index: ode/src/joints/pu.cpp
===================================================================
--- ode/src/joints/pu.cpp	(revision 1903)
+++ ode/src/joints/pu.cpp	(working copy)
@@ -209,7 +209,10 @@
 void 
 dxJointPU::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 3 +
+    limotP.countSureMaxRows() +
+    limot1.countSureMaxRows() +
+    limot2.countSureMaxRows();
 }
 
 
@@ -232,7 +235,8 @@
         limotP.testRotationalLimit( pos );  // N.B. The function is ill named
     }
 
-    if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+    info->m+=limotP.countRows();
+    info->nub+=limotP.countUBRows();
 
 
     bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
@@ -254,8 +258,13 @@
             limot2.testRotationalLimit( angle2 );
     }
 
-    if ( limot1.limit || limot1.fmax > 0 ) info->m++;
-    if ( limot2.limit || limot2.fmax > 0 ) info->m++;
+    //if ( limot1.limit || limot1.fmax > 0 ) info->m++;
+    //if ( limot2.limit || limot2.fmax > 0 ) info->m++;
+
+    info->m   += limot1.countRows();
+    info->nub += limot1.countUBRows();
+    info->m   += limot2.countRows();
+    info->nub += limot2.countUBRows();
 }
 
 
@@ -434,17 +443,17 @@
     info->c[1] = k * dCalcVectorDot3( ax1, err );
     info->c[2] = k * dCalcVectorDot3( q, err );
 
-    int row = 3 + limot1.addLimot( this, info, 3, ax1, 1 );
-    row += limot2.addLimot( this, info, row, ax2, 1 );
+    int row = 3 + limot1.addRotationalLimot( this, info,  ax1, 3 );
+    row += limot2.addRotationalLimot( this, info, ax2, row );
 
     if (  node[1].body || !(flags & dJOINT_REVERSE) )
-        limotP.addLimot( this, info, row, axP, 0 );
+        limotP.addLinearLimot( this, info, axP, row );
     else
     {
         axP[0] = -axP[0];
         axP[1] = -axP[1];
         axP[2] = -axP[2];
-        limotP.addLimot ( this, info, row, axP, 0 );
+        limotP.addLinearLimot ( this, info, axP, row );
     }
 }
 
Index: ode/src/joints/slider.cpp
===================================================================
--- ode/src/joints/slider.cpp	(revision 1903)
+++ ode/src/joints/slider.cpp	(working copy)
@@ -109,7 +109,7 @@
 void 
 dxJointSlider::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 5 + limot.countSureMaxRows();
 }
 
 
@@ -117,12 +117,8 @@
 dxJointSlider::getInfo1 ( dxJoint::Info1 *info )
 {
     info->nub = 5;
+    info->m = 5;
 
-    // see if joint is powered
-    if ( limot.fmax > 0 )
-        info->m = 6; // powered slider needs an extra constraint row
-    else info->m = 5;
-
     // see if we're at a joint limit.
     limot.limit = 0;
     if ( ( limot.lostop > -dInfinity || limot.histop < dInfinity ) &&
@@ -130,18 +126,9 @@
     {
         // measure joint position
         dReal pos = dJointGetSliderPosition ( this );
-        if ( pos <= limot.lostop )
-        {
-            limot.limit = 1;
-            limot.limit_err = pos - limot.lostop;
-            info->m = 6;
-        }
-        else if ( pos >= limot.histop )
-        {
-            limot.limit = 2;
-            limot.limit_err = pos - limot.histop;
-            info->m = 6;
-        }
+        limot.testRotationalLimit( pos );
+        info->m+=limot.countRows();
+        info->nub+=limot.countUBRows();
     }
 }
 
@@ -224,7 +211,7 @@
     }
 
     // if the slider is powered, or has joint limits, add in the extra row
-    limot.addLimot ( this, info, 5, ax1, 0 );
+    limot.addLinearLimot ( this, info, ax1, 5 );
 }
 
 
Index: ode/src/joints/universal.cpp
===================================================================
--- ode/src/joints/universal.cpp	(revision 1903)
+++ ode/src/joints/universal.cpp	(working copy)
@@ -258,7 +258,9 @@
 void 
 dxJointUniversal::getSureMaxInfo( SureMaxInfo* info )
 {
-    info->max_m = 6;
+  info->max_m = 4 +
+    limot1.countSureMaxRows() +
+    limot2.countSureMaxRows();
 }
 
 
@@ -288,8 +290,8 @@
             limot2.testRotationalLimit( angle2 );
     }
 
-    if ( limot1.limit || limot1.fmax > 0 ) info->m++;
-    if ( limot2.limit || limot2.fmax > 0 ) info->m++;
+    info->m += limot1.countRows() + limot2.countRows();
+    info->nub += limot1.countUBRows() + limot2.countUBRows();
 }
 
 
@@ -355,10 +357,10 @@
     info->c[3] = info->fps * info->erp * - k;
 
     // if the first angle is powered, or has joint limits, add in the stuff
-    int row = 4 + limot1.addLimot( this, info, 4, ax1, 1 );
+    int row = 4 + limot1.addRotationalLimot( this, info, ax1, 4 );
 
     // if the second angle is powered, or has joint limits, add in more stuff
-    limot2.addLimot( this, info, row, ax2, 1 );
+    limot2.addRotationalLimot( this, info, ax2, row );
 }
 
 
Index: ode/src/ode.cpp
===================================================================
--- ode/src/ode.cpp	(revision 1903)
+++ ode/src/ode.cpp	(working copy)
@@ -1269,7 +1269,19 @@
     return createJoint<dxJointDHinge> (w,group);
 }
 
+dxJoint * dJointCreateLMPlus (dWorldID w, dJointGroupID group)
+{
+    dAASSERT (w);
+    return createJoint<dxJointLMPlus> (w,group);
+}
 
+dxJoint * dJointCreateAMPlus (dWorldID w, dJointGroupID group)
+{
+    dAASSERT (w);
+    return createJoint<dxJointAMPlus> (w,group);
+}
+
+
 static void FinalizeAndDestroyJointInstance(dxJoint *j, bool delete_it)
 {
     // if any group joints have their world pointer set to 0, their world was
Index: ode/src/odemath.cpp
===================================================================
--- ode/src/odemath.cpp	(revision 1903)
+++ ode/src/odemath.cpp	(working copy)
@@ -177,7 +177,26 @@
     }
 }
 
+int dDirectedPlaneSpace (const dVector3 n, const dVector3 d, dVector3 p, dVector3 q)
+{
+    // Find the projection of d onto n (n assumed to be unit length)
+    dReal dotp = dCalcVectorDot3(n,d);
+    // Remove that projection from d and store the result in p
+    dAddScaledVectors3(p,d,n,1,-dotp);
 
+    if (p[0]==0 && p[1]==0 && p[2]==0) {
+        // If p==0, then compute a regular planeSpace
+        dPlaneSpace(n,p,q);
+        return 0;
+    } else {
+        // Otherwise, normalize p and cross with n for q
+        dNormalize3(p);
+        // If n and p are unit length, so should be q.
+        dCalcVectorCross3(q,n,p);
+        return 1;
+    }
+}
+
 /*
 * This takes what is supposed to be a rotation matrix,
 * and make sure it is correct.
Index: ode/src/quickstep.cpp
===================================================================
--- ode/src/quickstep.cpp	(revision 1903)
+++ ode/src/quickstep.cpp	(working copy)
@@ -683,7 +683,7 @@
         for (dxJoint *const *_jcurr = _joint; _jcurr != _jend; _jcurr++) {	// jicurr=dest, _jcurr=src
             dxJoint *j = *_jcurr;
             j->getInfo1 (&jicurr->info);
-            dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 6 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
+            dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 18 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
 
             unsigned int jm = jicurr->info.m;
             if (jm != 0) {
@@ -979,7 +979,7 @@
             dReal *vel = memarena->AllocateArray<dReal>((size_t)nb*6);
 
             // check that the updated velocity obeys the constraint (this check needs unmodified J)
-            dReal *velcurr = vel;
+            dReal *velcurr = lo_vel;
             dxBody *bodycurr = body, *const bodyend = body + nb;
             for (; bodycurr != bodyend; velcurr += 6, bodycurr++) {
                 for (unsigned int j=0; j<3; j++) {
Index: ode/src/step.cpp
===================================================================
--- ode/src/step.cpp	(revision 1903)
+++ ode/src/step.cpp	(working copy)
@@ -308,7 +308,7 @@
                     }
                     dxJoint *j = *_jcurr++;
                     j->getInfo1 (&jicurr->info);
-                    dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 6 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
+                    dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 18 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
                     if (jicurr->info.m != 0) {
                         mcurr += jicurr->info.m;
                         if (jicurr->info.nub == 0) { // A lcp info - a correct guess!!!
@@ -362,7 +362,7 @@
                     }
                     dxJoint *j = *_jcurr++;
                     j->getInfo1 (&jicurr->info);
-                    dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 6 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
+                    dIASSERT (/*jicurr->info.m >= 0 && */jicurr->info.m <= 18 && /*jicurr->info.nub >= 0 && */jicurr->info.nub <= jicurr->info.m);
                     if (jicurr->info.m != 0) {
                         mcurr += jicurr->info.m;
                         if (jicurr->info.nub == jicurr->info.m) { // An unbounded info - a correct guess!!!
