Menu
▾
▴
[Opende-svn] SF.net SVN: opende: [1035] trunk
|
From: <hid...@us...> - 2006-08-09 17:54:33
|
Revision: 1035 Author: hidden_asbestos Date: 2006-08-09 10:54:15 -0700 (Wed, 09 Aug 2006) ViewCVS: http://svn.sourceforge.net/opende/?rev=1035&view=rev Log Message: ----------- Integrated the plane-2d joint which will attempt to constrain an attached body to the z == 0 plane. Modified Paths: -------------- trunk/CHANGELOG.txt trunk/build/tests.lua trunk/include/ode/common.h trunk/include/ode/objects.h trunk/ode/src/joint.cpp trunk/ode/src/joint.h trunk/ode/src/ode.cpp Added Paths: ----------- trunk/ode/test/test_plane2d.cpp Modified: trunk/CHANGELOG.txt =================================================================== --- trunk/CHANGELOG.txt 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/CHANGELOG.txt 2006-08-09 17:54:15 UTC (rev 1035) @@ -9,6 +9,10 @@ ------------------------------------------------------------------------------ +08/09/06 david + + * Integrated plane2d joint type which constrains bodies to z == 0. + 07/06/06 david * Added heightfield primitive collision code. Simple test available in Modified: trunk/build/tests.lua =================================================================== --- trunk/build/tests.lua 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/build/tests.lua 2006-08-09 17:54:15 UTC (rev 1035) @@ -17,6 +17,7 @@ "joints", "motor", "ode", + "plane2d", "slider", "space", "space_stress", Modified: trunk/include/ode/common.h =================================================================== --- trunk/include/ode/common.h 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/include/ode/common.h 2006-08-09 17:54:15 UTC (rev 1035) @@ -138,7 +138,7 @@ #elif defined(HAVE_ISNANF) #define dIsNan(x) (isnanf(x)) #else - /* + /* fall back to _isnan which is the VC way, this may seem redundant since we already checked for _isnan before, but if isnan is detected by @@ -254,7 +254,8 @@ dJointTypeFixed, dJointTypeNull, dJointTypeAMotor, - dJointTypeLMotor + dJointTypeLMotor, + dJointTypePlane2D }; Modified: trunk/include/ode/objects.h =================================================================== --- trunk/include/ode/objects.h 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/include/ode/objects.h 2006-08-09 17:54:15 UTC (rev 1035) @@ -34,13 +34,13 @@ /** * @defgroup world World * - * The world object is a container for rigid bodies and joints. Objects in - * different worlds can not interact, for example rigid bodies from two + * The world object is a container for rigid bodies and joints. Objects in + * different worlds can not interact, for example rigid bodies from two * different worlds can not collide. * - * All the objects in a world exist at the same point in time, thus one + * All the objects in a world exist at the same point in time, thus one * reason to use separate worlds is to simulate systems at different rates. - * Most applications will only need one world. + * Most applications will only need one world. */ @@ -53,10 +53,10 @@ /** - * @brief Destroy a world and everything in it. + * @brief Destroy a world and everything in it. * - * This includes all bodies, and all joints that are not part of a joint - * group. Joints that are part of a joint group will be deactivated, and + * This includes all bodies, and all joints that are not part of a joint + * group. Joints that are part of a joint group will be deactivated, and * can be destroyed by calling, for example, dJointGroupEmpty(). * @ingroup world * @param world the identifier for the world the be destroyed. @@ -65,9 +65,9 @@ /** - * @brief Set the world's global gravity vector. + * @brief Set the world's global gravity vector. * - * The units are m/s^2, so Earth's gravity vector would be (0,0,-9.81), + * The units are m/s^2, so Earth's gravity vector would be (0,0,-9.81), * assuming that +z is up. The default is no gravity, i.e. (0,0,0). * * @ingroup world @@ -83,7 +83,7 @@ /** - * @brief Set the global ERP value, that controls how much error + * @brief Set the global ERP value, that controls how much error * correction is performed in each time step. * @ingroup world * @param dWorldID the identifier of the world. @@ -120,7 +120,7 @@ * @brief Step the world. * * This uses a "big matrix" method that takes time on the order of m^3 - * and memory on the order of m^2, where m is the total number of constraint + * and memory on the order of m^2, where m is the total number of constraint * rows. For large systems this will use a lot of memory and can be very slow, * but this is currently the most accurate method. * @ingroup world @@ -143,7 +143,7 @@ * computation may depend on integrator parameters that are set as * properties of the world. */ -ODE_API void dWorldImpulseToForce +ODE_API void dWorldImpulseToForce ( dWorldID, dReal stepsize, dReal ix, dReal iy, dReal iz, dVector3 force @@ -155,12 +155,12 @@ * @ingroup world * @remarks * This uses an iterative method that takes time on the order of m*N - * and memory on the order of m, where m is the total number of constraint + * and memory on the order of m, where m is the total number of constraint * rows N is the number of iterations. * For large systems this is a lot faster than dWorldStep(), * but it is less accurate. * @remarks - * QuickStep is great for stacks of objects especially when the + * QuickStep is great for stacks of objects especially when the * auto-disable feature is used as well. * However, it has poor accuracy for near-singular systems. * Near-singular systems can occur when using high-friction contacts, motors, @@ -171,7 +171,7 @@ * \li Increase CFM. * \li Reduce the number of contacts in your system (e.g. use the minimum * number of contacts for the feet of a robot or creature). - * \li Don't use excessive friction in the contacts. + * \li Don't use excessive friction in the contacts. * \li Use contact slip if appropriate * \li Avoid kinematic loops (however, kinematic loops are inevitable in * legged creatures). @@ -189,7 +189,7 @@ * step. * @ingroup world * @remarks - * More iterations will give a more accurate solution, but will take + * More iterations will give a more accurate solution, but will take * longer to compute. * @param num The default is 20 iterations. */ @@ -221,7 +221,7 @@ /* World contact parameter functions */ /** - * @brief Set the maximum correcting velocity that contacts are allowed + * @brief Set the maximum correcting velocity that contacts are allowed * to generate. * @ingroup world * @param vel The default value is infinity (i.e. no limit). @@ -244,8 +244,8 @@ * Contacts are allowed to sink into the surface layer up to the given * depth before coming to rest. * @param depth The default value is zero. - * @remarks - * Increasing this to some small value (e.g. 0.001) can help prevent + * @remarks + * Increasing this to some small value (e.g. 0.001) can help prevent * jittering problems due to contacts being repeatedly made and broken. */ ODE_API void dWorldSetContactSurfaceLayer (dWorldID, dReal depth); @@ -271,27 +271,27 @@ /** * @defgroup disable Automatic Enabling and Disabling * - * Every body can be enabled or disabled. Enabled bodies participate in the - * simulation, while disabled bodies are turned off and do not get updated + * Every body can be enabled or disabled. Enabled bodies participate in the + * simulation, while disabled bodies are turned off and do not get updated * during a simulation step. New bodies are always created in the enabled state. * - * A disabled body that is connected through a joint to an enabled body will be + * A disabled body that is connected through a joint to an enabled body will be * automatically re-enabled at the next simulation step. * - * Disabled bodies do not consume CPU time, therefore to speed up the simulation - * bodies should be disabled when they come to rest. This can be done automatically + * Disabled bodies do not consume CPU time, therefore to speed up the simulation + * bodies should be disabled when they come to rest. This can be done automatically * with the auto-disable feature. * - * If a body has its auto-disable flag turned on, it will automatically disable + * If a body has its auto-disable flag turned on, it will automatically disable * itself when * @li It has been idle for a given number of simulation steps. - * @li It has also been idle for a given amount of simulation time. + * @li It has also been idle for a given amount of simulation time. * - * A body is considered to be idle when the magnitudes of both its linear velocity + * A body is considered to be idle when the magnitudes of both its linear velocity * and angular velocity are below given thresholds. * - * Thus, every body has five auto-disable parameters: an enabled flag, a idle step - * count, an idle time, and linear/angular velocity thresholds. Newly created bodies + * Thus, every body has five auto-disable parameters: an enabled flag, a idle step + * count, an idle time, and linear/angular velocity thresholds. Newly created bodies * get these parameters from world. */ @@ -382,34 +382,34 @@ /** * @defgroup bodies Rigid Bodies * - * A rigid body has various properties from the point of view of the + * A rigid body has various properties from the point of view of the * simulation. Some properties change over time: * - * @li Position vector (x,y,z) of the body's point of reference. + * @li Position vector (x,y,z) of the body's point of reference. * Currently the point of reference must correspond to the body's center of mass. * @li Linear velocity of the point of reference, a vector (vx,vy,vz). - * @li Orientation of a body, represented by a quaternion (qs,qx,qy,qz) or + * @li Orientation of a body, represented by a quaternion (qs,qx,qy,qz) or * a 3x3 rotation matrix. - * @li Angular velocity vector (wx,wy,wz) which describes how the orientation - * changes over time. + * @li Angular velocity vector (wx,wy,wz) which describes how the orientation + * changes over time. * * Other body properties are usually constant over time: * * @li Mass of the body. - * @li Position of the center of mass with respect to the point of reference. - * In the current implementation the center of mass and the point of + * @li Position of the center of mass with respect to the point of reference. + * In the current implementation the center of mass and the point of * reference must coincide. - * @li Inertia matrix. This is a 3x3 matrix that describes how the body's mass - * is distributed around the center of mass. Conceptually each body has an + * @li Inertia matrix. This is a 3x3 matrix that describes how the body's mass + * is distributed around the center of mass. Conceptually each body has an * x-y-z coordinate frame embedded in it that moves and rotates with the body. * - * The origin of this coordinate frame is the body's point of reference. Some values - * in ODE (vectors, matrices etc) are relative to the body coordinate frame, and others + * The origin of this coordinate frame is the body's point of reference. Some values + * in ODE (vectors, matrices etc) are relative to the body coordinate frame, and others * are relative to the global coordinate frame. * - * Note that the shape of a rigid body is not a dynamical property (except insofar as - * it influences the various mass properties). It is only collision detection that cares - * about the detailed shape of the body. + * Note that the shape of a rigid body is not a dynamical property (except insofar as + * it influences the various mass properties). It is only collision detection that cares + * about the detailed shape of the body. */ @@ -527,9 +527,9 @@ /** * @brief Set position of a body. - * @remarks + * @remarks * After setting, the outcome of the simulation is undefined - * if the new configuration is inconsistent with the joints/constraints + * if the new configuration is inconsistent with the joints/constraints * that are present. * @ingroup bodies */ @@ -538,9 +538,9 @@ /** * @brief Set the orientation of a body. * @ingroup bodies - * @remarks + * @remarks * After setting, the outcome of the simulation is undefined - * if the new configuration is inconsistent with the joints/constraints + * if the new configuration is inconsistent with the joints/constraints * that are present. */ ODE_API void dBodySetRotation (dBodyID, const dMatrix3 R); @@ -548,9 +548,9 @@ /** * @brief Set the orientation of a body. * @ingroup bodies - * @remarks + * @remarks * After setting, the outcome of the simulation is undefined - * if the new configuration is inconsistent with the joints/constraints + * if the new configuration is inconsistent with the joints/constraints * that are present. */ ODE_API void dBodySetQuaternion (dBodyID, const dQuaternion q); @@ -572,7 +572,7 @@ * @ingroup bodies * @remarks * When getting, the returned values are pointers to internal data structures, - * so the vectors are valid until any changes are made to the rigid body + * so the vectors are valid until any changes are made to the rigid body * system structure. */ ODE_API const dReal * dBodyGetPosition (dBodyID); @@ -667,9 +667,9 @@ /** * @brief Return the current accumulated force vector. * @return points to an array of 3 reals. - * @remarks - * The returned values are pointers to internal data structures, so - * the vectors are only valid until any changes are made to the rigid + * @remarks + * The returned values are pointers to internal data structures, so + * the vectors are only valid until any changes are made to the rigid * body system. * @ingroup bodies */ @@ -678,9 +678,9 @@ /** * @brief Return the current accumulated torque vector. * @return points to an array of 3 reals. - * @remarks - * The returned values are pointers to internal data structures, so - * the vectors are only valid until any changes are made to the rigid + * @remarks + * The returned values are pointers to internal data structures, so + * the vectors are only valid until any changes are made to the rigid * body system. * @ingroup bodies */ @@ -688,7 +688,7 @@ /** * @brief Set the body force accumulation vector. - * @remarks + * @remarks * This is mostly useful to zero the force and torque for deactivated bodies * before they are reactivated, in the case where the force-adding functions * were called on them while they were deactivated. @@ -698,7 +698,7 @@ /** * @brief Set the body torque accumulation vector. - * @remarks + * @remarks * This is mostly useful to zero the force and torque for deactivated bodies * before they are reactivated, in the case where the force-adding functions * were called on them while they were deactivated. @@ -711,7 +711,7 @@ * @ingroup bodies * @param result will contain the result. */ -ODE_API void dBodyGetRelPointPos +ODE_API void dBodyGetRelPointPos ( dBodyID, dReal px, dReal py, dReal pz, dVector3 result @@ -729,7 +729,7 @@ ); /** - * @brief Get velocity vector in global coords of a globally + * @brief Get velocity vector in global coords of a globally * specified point on a body. * @ingroup bodies * @param result will contain the result. @@ -748,7 +748,7 @@ * @ingroup bodies * @param result will contain the result. */ -ODE_API void dBodyGetPosRelPoint +ODE_API void dBodyGetPosRelPoint ( dBodyID, dReal px, dReal py, dReal pz, dVector3 result @@ -770,7 +770,7 @@ * @ingroup bodies * @param result will contain the result. */ -ODE_API void dBodyVectorFromWorld +ODE_API void dBodyVectorFromWorld ( dBodyID, dReal px, dReal py, dReal pz, dVector3 result @@ -800,10 +800,10 @@ * @ingroup bodies * @remarks * This is axis only has meaning when the finite rotation mode is set - * If this axis is zero (0,0,0), full finite rotations are performed on + * If this axis is zero (0,0,0), full finite rotations are performed on * the body. * If this axis is nonzero, the body is rotated by performing a partial finite - * rotation along the axis direction followed by an infinitesimal rotation + * rotation along the axis direction followed by an infinitesimal rotation * along an orthogonal direction. * @remarks * This can be useful to alleviate certain sources of error caused by quickly @@ -1026,6 +1026,13 @@ */ ODE_API dJointID dJointCreateLMotor (dWorldID, dJointGroupID); +/** + * @brief Create a new joint of the plane-2d 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 dJointCreatePlane2D (dWorldID, dJointGroupID); /** * @brief Destroy a joint. @@ -1048,7 +1055,7 @@ /** * @brief Destroy a joint group. * @ingroup joints - * + * * All joints in the joint group will be destroyed. */ ODE_API void dJointGroupDestroy (dJointGroupID); @@ -1121,7 +1128,7 @@ * @brief Sets the datastructure that is to receive the feedback. * * The feedback can be used by the user, so that it is known how - * much force an individual joint exerts. + * much force an individual joint exerts. * @ingroup joints */ ODE_API void dJointSetFeedback (dJointID, dJointFeedback *); @@ -1132,8 +1139,8 @@ */ ODE_API dJointFeedback *dJointGetFeedback (dJointID); -/** - * @brief Set the joint anchor point. +/** + * @brief Set the joint anchor point. * @ingroup joints * * The joint will try to keep this point on each body @@ -1141,8 +1148,8 @@ */ ODE_API void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z); -/** - * @brief Set the joint anchor point. +/** + * @brief Set the joint anchor point. * @ingroup joints */ ODE_API void dJointSetBallAnchor2 (dJointID, dReal x, dReal y, dReal z); @@ -1170,8 +1177,8 @@ /** * @brief Applies the torque about the hinge axis. * - * That is, it applies a torque with specified magnitude in the direction - * of the hinge axis, to body 1, and with the same magnitude but in opposite + * That is, it applies a torque with specified magnitude in the direction + * of the hinge 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 */ @@ -1195,10 +1202,10 @@ ODE_API void dJointSetSliderParam (dJointID, int parameter, dReal value); /** - * @brief Applies the given force in the slider's direction. - * + * @brief Applies the given force in the slider's direction. + * * That is, it applies a force with specified magnitude, in the direction of - * slider's axis, to body1, and with the same magnitude but opposite + * slider's axis, to body1, and with the same magnitude but opposite * direction to body2. This function is just a wrapper for dBodyAddForce(). * @ingroup joints */ @@ -1229,7 +1236,7 @@ ODE_API void dJointSetHinge2Param (dJointID, int parameter, dReal value); /** - * @brief Applies torque1 about the hinge2's axis 1, torque2 about the + * @brief Applies torque1 about the hinge2's axis 1, torque2 about the * hinge2's axis 2. * @remarks This function is just a wrapper for dBodyAddTorque(). * @ingroup joints @@ -1269,8 +1276,8 @@ ODE_API void dJointAddUniversalTorques(dJointID joint, dReal torque1, dReal torque2); /** - * @brief Call this on the fixed joint after it has been attached to - * remember the current desired relative offset and desired relative + * @brief Call this on the fixed joint after it has been attached to + * remember the current desired relative offset and desired relative * rotation between the bodies. * @ingroup joints */ @@ -1292,7 +1299,7 @@ /** * @brief Tell the AMotor what the current angle is along axis anum. - * + * * This function should only be called in dAMotorUser mode, because in this * mode the AMotor has no other way of knowing the joint angles. * The angle information is needed if stops have been set along the axis, @@ -1314,10 +1321,10 @@ ODE_API void dJointSetAMotorMode (dJointID, int mode); /** - * @brief Applies torque0 about the AMotor's axis 0, torque1 about the + * @brief Applies torque0 about the AMotor's axis 0, torque1 about the * AMotor's axis 1, and torque2 about the AMotor's axis 2. - * @remarks - * If the motor has fewer than three axes, the higher torques are ignored. + * @remarks + * If the motor has fewer than three axes, the higher torques are ignored. * This function is just a wrapper for dBodyAddTorque(). * @ingroup joints */ @@ -1337,7 +1344,7 @@ * \li 0: The axis is anchored to the global frame. * \li 1: The axis is anchored to the first body. * \li 2: The axis is anchored to the second body. - * @remarks The axis vector is always specified in global coordinates + * @remarks The axis vector is always specified in global coordinates * regardless of the setting of rel. * @ingroup joints */ @@ -1350,9 +1357,25 @@ ODE_API void dJointSetLMotorParam (dJointID, int parameter, dReal value); /** + * @ingroup joints + */ +ODE_API void dJointSetPlane2DXParam (dJointID, int parameter, dReal value); + +/** + * @ingroup joints + */ + +ODE_API void dJointSetPlane2DYParam (dJointID, int parameter, dReal value); + +/** + * @ingroup joints + */ +ODE_API void dJointSetPlane2DAngleParam (dJointID, int parameter, dReal value); + +/** * @brief Get the joint anchor point, in world coordinates. * - * This returns the point on body 1. If the joint is perfectly satisfied, + * This returns the point on body 1. If the joint is perfectly satisfied, * this will be the same as the point on body 2. */ ODE_API void dJointGetBallAnchor (dJointID, dVector3 result); @@ -1360,11 +1383,11 @@ /** * @brief Get the joint anchor point, in world coordinates. * - * This returns the point on body 2. You can think of a ball and socket - * joint as trying to keep the result of dJointGetBallAnchor() and + * This returns the point on body 2. You can think of a ball and socket + * joint as trying to keep the result of dJointGetBallAnchor() and * dJointGetBallAnchor2() the same. If the joint is perfectly satisfied, * this function will return the same value as dJointGetBallAnchor() to - * within roundoff errors. dJointGetBallAnchor2() can be used, along with + * within roundoff errors. dJointGetBallAnchor2() can be used, along with * dJointGetBallAnchor(), to see how far the joint has come apart. */ ODE_API void dJointGetBallAnchor2 (dJointID, dVector3 result); @@ -1372,15 +1395,15 @@ /** * @brief Get the hinge anchor point, in world coordinates. * - * This returns the point on body 1. If the joint is perfectly satisfied, + * This returns the point on body 1. If the joint is perfectly satisfied, * this will be the same as the point on body 2. * @ingroup joints */ ODE_API void dJointGetHingeAnchor (dJointID, dVector3 result); /** - * @brief Get the joint anchor point, in world coordinates. - * @return The point on body 2. If the joint is perfectly satisfied, + * @brief Get the joint anchor point, in world coordinates. + * @return The point on body 2. If the joint is perfectly satisfied, * this will return the same value as dJointGetHingeAnchor(). * If not, this value will be slightly different. * This can be used, for example, to see how far the joint has come apart. @@ -1446,8 +1469,8 @@ ODE_API dReal dJointGetSliderParam (dJointID, int parameter); /** - * @brief Get the joint anchor point, in world coordinates. - * @return the point on body 1. If the joint is perfectly satisfied, + * @brief Get the joint anchor point, in world coordinates. + * @return the point on body 1. If the joint is perfectly satisfied, * this will be the same as the point on body 2. * @ingroup joints */ @@ -1455,7 +1478,7 @@ /** * @brief Get the joint anchor point, in world coordinates. - * This returns the point on body 2. If the joint is perfectly satisfied, + * This returns the point on body 2. If the joint is perfectly satisfied, * this will return the same value as dJointGetHinge2Anchor. * If not, this value will be slightly different. * This can be used, for example, to see how far the joint has come apart. @@ -1501,22 +1524,22 @@ /** * @brief Get the joint anchor point, in world coordinates. - * @return the point on body 1. If the joint is perfectly satisfied, + * @return the point on body 1. If the joint is perfectly satisfied, * this will be the same as the point on body 2. * @ingroup joints */ ODE_API void dJointGetUniversalAnchor (dJointID, dVector3 result); /** - * @brief Get the joint anchor point, in world coordinates. - * @return This returns the point on body 2. + * @brief Get the joint anchor point, in world coordinates. + * @return This returns the point on body 2. * @remarks * You can think of the ball and socket part of a universal joint as - * trying to keep the result of dJointGetBallAnchor() and + * trying to keep the result of dJointGetBallAnchor() and * dJointGetBallAnchor2() the same. If the joint is * perfectly satisfied, this function will return the same value - * as dJointGetUniversalAnchor() to within roundoff errors. - * dJointGetUniversalAnchor2() can be used, along with + * as dJointGetUniversalAnchor() to within roundoff errors. + * dJointGetUniversalAnchor2() can be used, along with * dJointGetUniversalAnchor(), to see how far the joint has come apart. * @ingroup joints */ @@ -1589,7 +1612,7 @@ /** * @brief Get axis * @remarks - * The axis vector is always specified in global coordinates regardless + * The axis vector is always specified in global coordinates regardless * of the setting of rel. * There are two GetAMotorAxis functions, one to return the axis and one to * return the relative mode. @@ -1631,7 +1654,7 @@ ODE_API dReal dJointGetAMotorParam (dJointID, int parameter); /** - * @brief Get the angular motor mode. + * @brief Get the angular motor mode. * @param mode must be one of the following constants: * \li dAMotorUser The AMotor axes and joint angle settings are entirely * controlled by the user. This is the default mode. @@ -1663,6 +1686,7 @@ */ ODE_API dReal dJointGetLMotorParam (dJointID, int parameter); + /** * @ingroup joints */ Modified: trunk/ode/src/joint.cpp =================================================================== --- trunk/ode/src/joint.cpp 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/ode/src/joint.cpp 2006-08-09 17:54:15 UTC (rev 1035) @@ -1187,7 +1187,7 @@ dUASSERT(joint,"bad joint argument"); dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); setAxes (joint,x,y,z,joint->axis1,0); - + // compute initial relative rotation body1 -> body2, or env -> body1 // also compute center of body1 w.r.t body 2 if (joint->node[1].body) { @@ -2760,7 +2760,7 @@ else if (joint->rel[i] == 2) { if (joint->node[1].body) { // jds: don't assert, just ignore dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); - } + } } else { ax[i][0] = joint->axis[i][0]; ax[i][1] = joint->axis[i][1]; @@ -2769,7 +2769,7 @@ } } -static void lmotorGetInfo1 (dxJointLMotor *j, dxJoint::Info1 *info) +static void lmotorGetInfo1 (dxJointLMotor *j, dxJoint::Info1 *info) { info->m = 0; info->nub = 0; @@ -2785,10 +2785,10 @@ int row=0; dVector3 ax[3]; lmotorComputeGlobalAxes(joint, ax); - - for (int i=0;i<joint->num;i++) { + + for (int i=0;i<joint->num;i++) { row += joint->limot[i].addLimot(joint,info,row,ax[i], 0); - } + } } void dJointSetLMotorAxis (dJointID j, int anum, int rel, dReal x, dReal y, dReal z) @@ -2813,7 +2813,7 @@ if (rel==1) { dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); } else { - //second body has to exists thanks to ref 1 line + //second body has to exists thanks to ref 1 line dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); } } else { @@ -2999,3 +2999,165 @@ (dxJoint::getInfo1_fn*) nullGetInfo1, (dxJoint::getInfo2_fn*) nullGetInfo2, dJointTypeNull}; + + + + +/* + This code is part of the Plane2D ODE joint + by ps...@gm... + Wed Apr 23 18:53:43 CEST 2003 + + Add this code to the file: ode/src/joint.cpp +*/ + + +# define VoXYZ(v1, o1, x, y, z) \ + ( \ + (v1)[0] o1 (x), \ + (v1)[1] o1 (y), \ + (v1)[2] o1 (z) \ + ) + +static dReal Midentity[3][3] = + { + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1, } + }; + + + +static void plane2dInit (dxJointPlane2D *j) +/*********************************************/ +{ + /* MINFO ("plane2dInit ()"); */ + j->motor_x.init (j->world); + j->motor_y.init (j->world); + j->motor_angle.init (j->world); +} + + + +static void plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info) +/***********************************************************************/ +{ + /* MINFO ("plane2dGetInfo1 ()"); */ + + info->nub = 3; + info->m = 3; + + if (j->motor_x.fmax > 0) + j->row_motor_x = info->m ++; + if (j->motor_y.fmax > 0) + j->row_motor_y = info->m ++; + if (j->motor_angle.fmax > 0) + j->row_motor_angle = info->m ++; +} + + + +static void plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info) +/***************************************************************************/ +{ + int r0 = 0, + r1 = info->rowskip, + r2 = 2 * r1; + dReal eps = info->fps * info->erp; + + /* MINFO ("plane2dGetInfo2 ()"); */ + +/* + v = v1, w = omega1 + (v2, omega2 not important (== static environment)) + + constraint equations: + xz = 0 + wx = 0 + wy = 0 + + <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 ) + ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 ) + ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 ) + J1/J1l Omega1/J1a +*/ + + // fill in linear and angular coeff. for left hand side: + + VoXYZ (&info->J1l[r0], =, 0, 0, 1); + VoXYZ (&info->J1l[r1], =, 0, 0, 0); + VoXYZ (&info->J1l[r2], =, 0, 0, 0); + + VoXYZ (&info->J1a[r0], =, 0, 0, 0); + VoXYZ (&info->J1a[r1], =, 1, 0, 0); + VoXYZ (&info->J1a[r2], =, 0, 1, 0); + + // error correction (against drift): + + // a) linear vz, so that z (== pos[2]) == 0 + info->c[0] = eps * -joint->node[0].body->posr.pos[2]; + +# if 0 + // b) angular correction? -> left to application !!! + dReal *body_z_axis = &joint->node[0].body->R[8]; + info->c[1] = eps * +atan2 (body_z_axis[1], body_z_axis[2]); // wx error + info->c[2] = eps * -atan2 (body_z_axis[0], body_z_axis[2]); // wy error +# endif + + // if the slider is powered, or has joint limits, add in the extra row: + + if (joint->row_motor_x > 0) + joint->motor_x.addLimot ( + joint, info, joint->row_motor_x, Midentity[0], 0); + + if (joint->row_motor_y > 0) + joint->motor_y.addLimot ( + joint, info, joint->row_motor_y, Midentity[1], 0); + + if (joint->row_motor_angle > 0) + joint->motor_angle.addLimot ( + joint, info, joint->row_motor_angle, Midentity[2], 1); +} + + + +dxJoint::Vtable __dplane2d_vtable = +{ + sizeof (dxJointPlane2D), + (dxJoint::init_fn*) plane2dInit, + (dxJoint::getInfo1_fn*) plane2dGetInfo1, + (dxJoint::getInfo2_fn*) plane2dGetInfo2, + dJointTypePlane2D +}; + + +void dJointSetPlane2DXParam (dxJoint *joint, + int parameter, dReal value) +{ + dUASSERT (joint, "bad joint argument"); + dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); + dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); + joint2d->motor_x.set (parameter, value); +} + + +void dJointSetPlane2DYParam (dxJoint *joint, + int parameter, dReal value) +{ + dUASSERT (joint, "bad joint argument"); + dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); + dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); + joint2d->motor_y.set (parameter, value); +} + + + +void dJointSetPlane2DAngleParam (dxJoint *joint, + int parameter, dReal value) +{ + dUASSERT (joint, "bad joint argument"); + dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d"); + dxJointPlane2D* joint2d = (dxJointPlane2D*)( joint ); + joint2d->motor_angle.set (parameter, value); +} + Modified: trunk/ode/src/joint.h =================================================================== --- trunk/ode/src/joint.h 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/ode/src/joint.h 2006-08-09 17:54:15 UTC (rev 1035) @@ -253,9 +253,25 @@ dVector3 axis[3]; dxJointLimitMotor limot[3]; }; - + extern struct dxJoint::Vtable __dlmotor_vtable; + +// 2d joint, constrains to z == 0 + +struct dxJointPlane2D : public dxJoint +{ + int row_motor_x; + int row_motor_y; + int row_motor_angle; + dxJointLimitMotor motor_x; + dxJointLimitMotor motor_y; + dxJointLimitMotor motor_angle; +}; + +extern struct dxJoint::Vtable __dplane2d_vtable; + + // fixed struct dxJointFixed : public dxJoint { @@ -272,5 +288,4 @@ extern struct dxJoint::Vtable __dnull_vtable; - #endif Modified: trunk/ode/src/ode.cpp =================================================================== --- trunk/ode/src/ode.cpp 2006-08-08 13:01:19 UTC (rev 1034) +++ trunk/ode/src/ode.cpp 2006-08-09 17:54:15 UTC (rev 1035) @@ -853,7 +853,7 @@ b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world); b->adis.idle_time = dWorldGetAutoDisableTime(b->world); } - else + else { b->flags |= dxBodyAutoDisable; } @@ -981,6 +981,12 @@ return createJoint (w,group,&__dlmotor_vtable); } +dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dplane2d_vtable); +} + void dJointDestroy (dxJoint *j) { dAASSERT (j); @@ -1147,7 +1153,7 @@ b1 = in_b1; b2 = in_b2; } - + // look through b1's neighbour list for b2 for (dxJointNode *n=b1->firstjoint; n; n=n->next) { if (n->body == b2) return n->joint; @@ -1173,7 +1179,7 @@ b1 = in_b1; b2 = in_b2; } - + // look through b1's neighbour list for b2 int numConnectingJoints = 0; for (dxJointNode *n=b1->firstjoint; n; n=n->next) { Added: trunk/ode/test/test_plane2d.cpp =================================================================== --- trunk/ode/test/test_plane2d.cpp (rev 0) +++ trunk/ode/test/test_plane2d.cpp 2006-08-09 17:54:15 UTC (rev 1035) @@ -0,0 +1,266 @@ +// Test my Plane2D constraint. +// Uses ode-0.35 collision API. + +# include <stdio.h> +# include <stdlib.h> +# include <math.h> +# include <ode/ode.h> +# include <drawstuff/drawstuff.h> + +# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX))) + +# define N_BODIES 40 +# define STAGE_SIZE 8.0 // in m + +# define TIME_STEP 0.01 +# define K_SPRING 10.0 +# define K_DAMP 10.0 + + +static dWorld dyn_world; +static dBody dyn_bodies[N_BODIES]; +static dReal bodies_sides[N_BODIES][3]; + +static dSpaceID coll_space_id; +static dJointID plane2d_joint_ids[N_BODIES]; +static dJointGroup + coll_contacts; + + + +static void cb_start () +/*************************/ +{ + static float xyz[3] = { 0.5*STAGE_SIZE, 0.5*STAGE_SIZE, 0.65*STAGE_SIZE}; + static float hpr[3] = { 90, -90, 0 }; + + dsSetViewpoint (xyz, hpr); +} + + + +static void cb_near_collision (void *data, dGeomID o1, dGeomID o2) +/********************************************************************/ +{ + dBodyID b1 = dGeomGetBody (o1); + dBodyID b2 = dGeomGetBody (o2); + dContact contact; + + + // exit without doing anything if the two bodies are static + if (b1 == 0 && b2 == 0) + return; + + // exit without doing anything if the two bodies are connected by a joint + if (b1 && b2 && dAreConnected (b1, b2)) + { + /* MTRAP; */ + return; + } + + contact.surface.mode = 0; + contact.surface.mu = 0; // frictionless + + if (dCollide (o1, o2, 0, &contact.geom, sizeof (dContactGeom))) + { + dJointID c = dJointCreateContact (dyn_world.id(), + coll_contacts.id (), &contact); + dJointAttach (c, b1, b2); + } +} + + +static void track_to_pos (dBody &body, dJointID joint_id, + dReal target_x, dReal target_y) +/************************************************************************/ +{ + double curr_x = body.getPosition()[0]; + double curr_y = body.getPosition()[1]; + + dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x)); + dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y)); +} + + + +static void cb_sim_step (int pause) +/*************************************/ +{ + if (! pause) + { + static double + angle = 0; + + angle += 0.01; + + track_to_pos (dyn_bodies[0], plane2d_joint_ids[0], + STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle), + STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle)); + + /* double f0 = 0.001; */ + /* for (int b = 0; b < N_BODIES; b ++) */ + /* { */ + /* double p = 1 + b / (double) N_BODIES; */ + /* double q = 2 - b / (double) N_BODIES; */ + /* dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */ + /* } */ + /* dyn_bodies[0].addTorque (0, 0, 0.1); */ + + const int n = 10; + for (int i = 0; i < n; i ++) + { + dSpaceCollide (coll_space_id, 0, &cb_near_collision); + dyn_world.step (TIME_STEP/n); + coll_contacts.empty (); + } + } + +# if 1 /* [ */ + { + // @@@ hack Plane2D constraint error reduction here: + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *rot = dBodyGetAngularVel (dyn_bodies[b].id()); + const dReal *quat_ptr; + dReal quat[4], + quat_len; + + + quat_ptr = dBodyGetQuaternion (dyn_bodies[b].id()); + quat[0] = quat_ptr[0]; + quat[1] = 0; + quat[2] = 0; + quat[3] = quat_ptr[3]; + quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]); + quat[0] /= quat_len; + quat[3] /= quat_len; + dBodySetQuaternion (dyn_bodies[b].id(), quat); + dBodySetAngularVel (dyn_bodies[b].id(), 0, 0, rot[2]); + } + } +# endif /* ] */ + + +# if 0 /* [ */ + { + // @@@ friction + for (int b = 0; b < N_BODIES; b ++) + { + const dReal *vel = dBodyGetLinearVel (dyn_bodies[b].id()), + *rot = dBodyGetAngularVel (dyn_bodies[b].id()); + dReal s = 1.00; + dReal t = 0.99; + + dBodySetLinearVel (dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]); + dBodySetAngularVel (dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]); + } + } +# endif /* ] */ + + + { + // ode drawstuff + + dsSetTexture (DS_WOOD); + for (int b = 0; b < N_BODIES; b ++) + { + if (b == 0) + dsSetColor (1.0, 0.5, 1.0); + else + dsSetColor (0, 0.5, 1.0); + /* if (b == 0) */ + /* dsDrawSphereD (dyn_bodies[b].getPosition(), */ + /* dyn_bodies[b].getRotation(), bodies_sides[b][0]); */ + /* else */ + dsDrawBox (dyn_bodies[b].getPosition(), + dyn_bodies[b].getRotation(), bodies_sides[b]); + } + } +} + + + +extern int main +/******************/ +( + int argc, + char **argv +) +{ + int b; + dsFunctions drawstuff_functions; + + + // dynamic world + + double cf_mixing = 1 / (TIME_STEP * K_SPRING + K_DAMP); + double err_reduct = TIME_STEP * K_SPRING * cf_mixing; + err_reduct = 0.5; + cf_mixing = 0.001; + dWorldSetERP (dyn_world.id (), err_reduct); + dWorldSetCFM (dyn_world.id (), cf_mixing); + dyn_world.setGravity (0, 0.0, -1.0); + + coll_space_id = dSimpleSpaceCreate (0); + + // dynamic bodies + for (b = 0; b < N_BODIES; b ++) + { + int l = (int) (1 + sqrt ((double) N_BODIES)); + double x = (0.5 + (b / l)) / l * STAGE_SIZE; + double y = (0.5 + (b % l)) / l * STAGE_SIZE; + double z = 1.0 + 0.1 * drand48 (); + + bodies_sides[b][0] = 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES); + bodies_sides[b][1] = 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES); + bodies_sides[b][2] = z; + + dyn_bodies[b].create (dyn_world); + dyn_bodies[b].setPosition (x, y, z/2); + dyn_bodies[b].setData ((void*) b); + dBodySetLinearVel (dyn_bodies[b].id (), + 3 * (drand48 () - 0.5), 3 * (drand48 () - 0.5), 0); + + dMass m; + m.setBox (1, bodies_sides[b][0],bodies_sides[b][1],bodies_sides[b][2]); + m.adjust (0.1 * bodies_sides[b][0] * bodies_sides[b][1]); + dyn_bodies[b].setMass (&m); + + plane2d_joint_ids[b] = dJointCreatePlane2D (dyn_world.id (), 0); + dJointAttach (plane2d_joint_ids[b], dyn_bodies[b].id (), 0); + } + + dJointSetPlane2DXParam (plane2d_joint_ids[0], dParamFMax, 10); + dJointSetPlane2DYParam (plane2d_joint_ids[0], dParamFMax, 10); + + + // collision geoms and joints + dCreatePlane (coll_space_id, 1, 0, 0, 0); + dCreatePlane (coll_space_id, -1, 0, 0, -STAGE_SIZE); + dCreatePlane (coll_space_id, 0, 1, 0, 0); + dCreatePlane (coll_space_id, 0, -1, 0, -STAGE_SIZE); + + for (b = 0; b < N_BODIES; b ++) + { + dGeomID coll_box_id; + coll_box_id = dCreateBox (coll_space_id, + bodies_sides[b][0], bodies_sides[b][1], bodies_sides[b][2]); + dGeomSetBody (coll_box_id, dyn_bodies[b].id ()); + } + + coll_contacts.create (0); + + { + // simulation loop (by drawstuff lib) + drawstuff_functions.version = DS_VERSION; + drawstuff_functions.start = &cb_start; + drawstuff_functions.step = &cb_sim_step; + drawstuff_functions.command = 0; + drawstuff_functions.stop = 0; + drawstuff_functions.path_to_textures = "../../drawstuff/textures"; + + dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions); + } + + return 0; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
