openrave: headers diff between 0.4.0 and 0.4.1 versions

	collisionchecker.h	collisionchecker.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2010 Rosen Diankov (rosen.diankov@gmail.com)	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 30	skipping to change at line 30
	#ifndef OPENRAVE_COLLISIONCHECKER_H	#ifndef OPENRAVE_COLLISIONCHECKER_H
	#define OPENRAVE_COLLISIONCHECKER_H	#define OPENRAVE_COLLISIONCHECKER_H

	namespace OpenRAVE {	namespace OpenRAVE {

	/// options for collision checker	/// options for collision checker
	enum CollisionOptions	enum CollisionOptions
	{	{
	CO_Distance = 1, ///< compute distance measurements, this is usually sl ow	CO_Distance = 1, ///< compute distance measurements, this is usually sl ow
	CO_UseTolerance = 2,	CO_UseTolerance = 2,

	CO_Contacts = 4, ///< Collision returns contact points	CO_Contacts = 4, ///< Collision returns contact points
	CO_RayAnyHit = 8, ///< when performing collision with rays, if this is set, algorithm just returns any hit instead of the closest (can be faster)	CO_RayAnyHit = 8, ///< when performing collision with rays, if this is set, algorithm just returns any hit instead of the closest (can be faster)
	CO_ActiveDOFs = 16, ///< if set and the target object is a robot, then only the links controlled by the currently set active DOFs will be checked for collisions. This allows planners to reduce redundant collision checks.	CO_ActiveDOFs = 16, ///< if set and the target object is a robot, then only the links controlled by the currently set active DOFs will be checked for collisions. This allows planners to reduce redundant collision checks.
	};	};


	/// action to perform whenever a collision is detected between objects	/// \brief action to perform whenever a collision is detected between objec ts
	enum CollisionAction	enum CollisionAction
	{	{
	CA_DefaultAction = 0, ///< let the physics/collision engine resolve the action	CA_DefaultAction = 0, ///< let the physics/collision engine resolve the action
	CA_Ignore = 1, ///< do nothing	CA_Ignore = 1, ///< do nothing
	};	};

	/// \brief Holds information about a particular collision that occured.	/// \brief Holds information about a particular collision that occured.
	class OPENRAVE_API CollisionReport	class OPENRAVE_API CollisionReport
	{	{
	public:	public:

	CollisionReport() { Reset(); }	CollisionReport() {
		Reset();
		}

	class OPENRAVE_API CONTACT	class OPENRAVE_API CONTACT
	{	{

	public:	public:
	CONTACT() : depth(0) {}	CONTACT() : depth(0) {
	CONTACT(const Vector& p, const Vector& n, dReal d) : pos(p), norm(n	}
	) {depth = d;}	CONTACT(const Vector &p, const Vector &n, dReal d) : pos(p), norm(n
		) {
		depth = d;
		}


	Vector pos; ///< where the contact occured	Vector pos; ///< where the contact occured
	Vector norm; ///< the normals of the faces	Vector norm; ///< the normals of the faces
	dReal depth; ///< the penetration depth, positive means the surf	dReal depth; ///< the penetration depth, positive means the surface
	aces are penetrating, negative means the surfaces are not colliding (used f	s are penetrating, negative means the surfaces are not colliding (used for
	or distance queries)	distance queries)
	};	};


	int options; ///< the options that the CollisionReport was calle d with	int options; ///< the options that the CollisionReport was called with

	KinBody::LinkConstPtr plink1, plink2; ///< the colliding links if a col lision involves a bodies. Collisions do not always occur with 2 bodies like ray collisions, so these fields can be empty.	KinBody::LinkConstPtr plink1, plink2; ///< the colliding links if a col lision involves a bodies. Collisions do not always occur with 2 bodies like ray collisions, so these fields can be empty.

	//KinBody::Link::GeomConstPtr pgeom1, pgeom2; ///< the specified geomet ries hit for the given links	//KinBody::Link::GeomConstPtr pgeom1, pgeom2; ///< the specified geomet ries hit for the given links

	int numCols; ///< this is the number of objects that collide with the object of interest	int numCols; ///< this is the number of objects that collide with the o bject of interest
	std::vector<KinBody::LinkConstPtr> vLinkColliding; ///< objects collidi ng with this object	std::vector<KinBody::LinkConstPtr> vLinkColliding; ///< objects collidi ng with this object


	dReal minDistance; ///< minimum distance from last query, filled i	dReal minDistance; ///< minimum distance from last query, filled if CO_
	f CO_Distance option is set	Distance option is set
	int numWithinTol; ///< number of objects within tolerance of this	int numWithinTol; ///< number of objects within tolerance of this objec
	object, filled if CO_UseTolerance option is set	t, filled if CO_UseTolerance option is set

	std::vector<CONTACT> contacts; ///< the convention is that the normal w ill be "out" of plink1's surface. Filled if CO_UseContacts option is set.	std::vector<CONTACT> contacts; ///< the convention is that the normal w ill be "out" of plink1's surface. Filled if CO_UseContacts option is set.

	virtual void Reset(int coloptions = 0);	virtual void Reset(int coloptions = 0);
	virtual std::string __str__() const;	virtual std::string __str__() const;
	};	};

	typedef CollisionReport COLLISIONREPORT RAVE_DEPRECATED;	typedef CollisionReport COLLISIONREPORT RAVE_DEPRECATED;

	/** \brief <b>[interface]</b> Responsible for all collision checking querie s of the environment. See \ref arch_collisionchecker.	/** \brief <b>[interface]</b> Responsible for all collision checking querie s of the environment. See \ref arch_collisionchecker.
	\ingroup interfaces	\ingroup interfaces

	*/	*/
	class OPENRAVE_API CollisionCheckerBase : public InterfaceBase	class OPENRAVE_API CollisionCheckerBase : public InterfaceBase
	{	{
	public:	public:

	CollisionCheckerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Collis	CollisionCheckerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Collis
	ionChecker, penv) {}	ionChecker, penv) {
	virtual ~CollisionCheckerBase() {}	}
		virtual ~CollisionCheckerBase() {
		}

	/// \brief return the static interface type this class points to (used for safe casting)	/// \brief return the static interface type this class points to (used for safe casting)

	static inline InterfaceType GetInterfaceTypeStatic() { return PT_Collis	static inline InterfaceType GetInterfaceTypeStatic() {
	ionChecker; }	return PT_CollisionChecker;
		}

	/// \brief Set basic collision options using the CollisionOptions enum	/// \brief Set basic collision options using the CollisionOptions enum
	virtual bool SetCollisionOptions(int collisionoptions) = 0;	virtual bool SetCollisionOptions(int collisionoptions) = 0;

	/// \brief get the current collision options	/// \brief get the current collision options
	virtual int GetCollisionOptions() const = 0;	virtual int GetCollisionOptions() const = 0;

	virtual void SetTolerance(dReal tolerance) = 0;	virtual void SetTolerance(dReal tolerance) = 0;

	/// notified when a new body has been initialized in the environment	/// notified when a new body has been initialized in the environment

	skipping to change at line 169	skipping to change at line 178

	/// called when environment switches to a different collision checker e ngine	/// called when environment switches to a different collision checker e ngine
	/// has to clear/deallocate any memory associated with KinBody::_pColli sionData	/// has to clear/deallocate any memory associated with KinBody::_pColli sionData
	virtual void DestroyEnvironment() = 0;	virtual void DestroyEnvironment() = 0;

	/// \brief Checks self collision only with the links of the passed in b ody.	/// \brief Checks self collision only with the links of the passed in b ody.
	///	///
	/// Links that are joined together are ignored.	/// Links that are joined together are ignored.
	virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportP tr report = CollisionReportPtr()) = 0;	virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportP tr report = CollisionReportPtr()) = 0;


	virtual void SetCollisionData(KinBodyPtr pbody, UserDataPtr data) {	virtual void SetCollisionData(KinBodyPtr pbody, UserDataPtr data) {
	pbody->SetCollisionData(data); }	pbody->SetCollisionData(data);
		}

	private:	private:

	virtual const char* GetHash() const { return OPENRAVE_COLLISIONCHECKER_	virtual const char* GetHash() const {
	HASH; }	return OPENRAVE_COLLISIONCHECKER_HASH;
		}

	#ifdef RAVE_PRIVATE	#ifdef RAVE_PRIVATE
	#ifdef _MSC_VER	#ifdef _MSC_VER
	friend class Environment;	friend class Environment;
	#else	#else
	friend class ::Environment;	friend class ::Environment;
	#endif	#endif
	#endif	#endif
	friend class KinBody;	friend class KinBody;
	};	};

	/// \brief Helper class to save and restore the collision options. If optio ns are not supported and required is true, throws an exception.	/// \brief Helper class to save and restore the collision options. If optio ns are not supported and required is true, throws an exception.
	class OPENRAVE_API CollisionOptionsStateSaver	class OPENRAVE_API CollisionOptionsStateSaver
	{	{

	public:	public:
	CollisionOptionsStateSaver(CollisionCheckerBasePtr p, int newoptions, b ool required=true);	CollisionOptionsStateSaver(CollisionCheckerBasePtr p, int newoptions, b ool required=true);
	virtual ~CollisionOptionsStateSaver();	virtual ~CollisionOptionsStateSaver();

	private:	private:
	int _oldoptions; ///< saved options	int _oldoptions; ///< saved options
	CollisionCheckerBasePtr _p;	CollisionCheckerBasePtr _p;
	};	};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	#endif	#endif

End of changes. 16 change blocks.
	32 lines changed or deleted	42 lines changed or added

	config.h	config.h

	skipping to change at line 40	skipping to change at line 40
	#define OPENRAVE_API OPENRAVE_HELPER_DLL_IMPORT	#define OPENRAVE_API OPENRAVE_HELPER_DLL_IMPORT
	#endif // OPENRAVE_DLL_EXPORTS	#endif // OPENRAVE_DLL_EXPORTS
	#define OPENRAVE_LOCAL OPENRAVE_HELPER_DLL_LOCAL	#define OPENRAVE_LOCAL OPENRAVE_HELPER_DLL_LOCAL
	#else // OPENRAVE_DLL is not defined: this means OpenRAVE is a static lib.	#else // OPENRAVE_DLL is not defined: this means OpenRAVE is a static lib.
	#define OPENRAVE_API	#define OPENRAVE_API
	#define OPENRAVE_LOCAL	#define OPENRAVE_LOCAL
	#endif // OPENRAVE_DLL	#endif // OPENRAVE_DLL

	#define OPENRAVE_VERSION_MAJOR 0	#define OPENRAVE_VERSION_MAJOR 0
	#define OPENRAVE_VERSION_MINOR 4	#define OPENRAVE_VERSION_MINOR 4

	#define OPENRAVE_VERSION_PATCH 0	#define OPENRAVE_VERSION_PATCH 1
	#define OPENRAVE_VERSION_COMBINED(major, minor, patch) (((major) << 16) \| ( (minor) << 8) \| (patch))	#define OPENRAVE_VERSION_COMBINED(major, minor, patch) (((major) << 16) \| ( (minor) << 8) \| (patch))
	#define OPENRAVE_VERSION OPENRAVE_VERSION_COMBINED(OPENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH)	#define OPENRAVE_VERSION OPENRAVE_VERSION_COMBINED(OPENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH)
	#define OPENRAVE_VERSION_EXTRACT_MAJOR(version) (((version)>>16)&0xff)	#define OPENRAVE_VERSION_EXTRACT_MAJOR(version) (((version)>>16)&0xff)
	#define OPENRAVE_VERSION_EXTRACT_MINOR(version) (((version)>>8)&0xff)	#define OPENRAVE_VERSION_EXTRACT_MINOR(version) (((version)>>8)&0xff)
	#define OPENRAVE_VERSION_EXTRACT_PATCH(version) (((version))&0xff)	#define OPENRAVE_VERSION_EXTRACT_PATCH(version) (((version))&0xff)

	#define OPENRAVE_VERSION_STRING "0.4.0"	#define OPENRAVE_VERSION_STRING "0.4.1"
	#define OPENRAVE_VERSION_STRING_FORMAT(version) boost::str(boost::format("% s.%s.%s")%(OPENRAVE_VERSION_EXTRACT_MAJOR(version))%(OPENRAVE_VERSION_EXTRA CT_MINOR(version))%(OPENRAVE_VERSION_EXTRACT_PATCH(version)))	#define OPENRAVE_VERSION_STRING_FORMAT(version) boost::str(boost::format("% s.%s.%s")%(OPENRAVE_VERSION_EXTRACT_MAJOR(version))%(OPENRAVE_VERSION_EXTRA CT_MINOR(version))%(OPENRAVE_VERSION_EXTRACT_PATCH(version)))

	#define OPENRAVE_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (OPENRAVE_VERSION_COMBINED(major1, minor1, patch1) >= OPENRAVE_VERSION_COM BINED(major2, minor2, patch2))	#define OPENRAVE_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (OPENRAVE_VERSION_COMBINED(major1, minor1, patch1) >= OPENRAVE_VERSION_COM BINED(major2, minor2, patch2))
	#define OPENRAVE_VERSION_MINIMUM(major, minor, patch) OPENRAVE_VERSION_GE(O PENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH, majo r, minor, patch)	#define OPENRAVE_VERSION_MINIMUM(major, minor, patch) OPENRAVE_VERSION_GE(O PENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH, majo r, minor, patch)

	// if 0, single precision	// if 0, single precision
	// if 1, double precision	// if 1, double precision
	#define OPENRAVE_PRECISION 1	#define OPENRAVE_PRECISION 1


	#define OPENRAVE_PLUGINS_INSTALL_DIR "/home/andrey/upstream-tracker/testing	#define OPENRAVE_PLUGINS_INSTALL_DIR "/home/andrey/upstream-tracker/testing
	/openrave/0.4.0/share/openrave-0.4/plugins"	/openrave/0.4.1/share/openrave-0.4/plugins"
	#define OPENRAVE_DATA_INSTALL_DIR "/home/andrey/upstream-tracker/testing/op	#define OPENRAVE_DATA_INSTALL_DIR "/home/andrey/upstream-tracker/testing/op
	enrave/0.4.0/share/openrave-0.4"	enrave/0.4.1/share/openrave-0.4"
	#define OPENRAVE_PYTHON_INSTALL_DIR "/home/andrey/upstream-tracker/testing/	#define OPENRAVE_PYTHON_INSTALL_DIR "/home/andrey/upstream-tracker/testing/
	openrave/0.4.0/lib/python2.7/site-packages"	openrave/0.4.1/lib/python2.7/site-packages"

	#endif	#endif

End of changes. 3 change blocks.
	8 lines changed or deleted	8 lines changed or added

	controller.h	controller.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2010 Rosen Diankov (rosen.diankov@gmail.com)	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	// GNU Lesser General Public License for more details.	// GNU Lesser General Public License for more details.
	//	//
	// You should have received a copy of the GNU Lesser General Public License	// You should have received a copy of the GNU Lesser General Public License
	// along with this program. If not, see <http://www.gnu.org/licenses/>.	// along with this program. If not, see <http://www.gnu.org/licenses/>.
	/** \file controller.h	/** \file controller.h
	\brief Controller related definitions.	\brief Controller related definitions.

	*/	*/
	#ifndef OPENRAVE_CONTROLLER_H	#ifndef OPENRAVE_CONTROLLER_H
	#define OPENRAVE_CONTROLLER_H	#define OPENRAVE_CONTROLLER_H

	namespace OpenRAVE {	namespace OpenRAVE {

	/** \brief <b>[interface]</b> Abstract base class to encapsulate a local co ntroller. <b>Methods not multi-thread safe.</b> See \ref arch_controller.	/** \brief <b>[interface]</b> Abstract base class to encapsulate a local co ntroller. <b>Methods not multi-thread safe.</b> See \ref arch_controller.
	\ingroup interfaces	\ingroup interfaces

	*/	*/
	class OPENRAVE_API ControllerBase : public InterfaceBase	class OPENRAVE_API ControllerBase : public InterfaceBase
	{	{
	public:	public:

	ControllerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Controller,	ControllerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Controller,
	penv) {}	penv) {
	virtual ~ControllerBase() {}	}
		virtual ~ControllerBase() {
		}

	/// \brief return the static interface type this class points to (used for safe casting)	/// \brief return the static interface type this class points to (used for safe casting)

	static inline InterfaceType GetInterfaceTypeStatic() { return PT_Contro	static inline InterfaceType GetInterfaceTypeStatic() {
	ller; }	return PT_Controller;
		}

	/// \brief Initializes the controller	/// \brief Initializes the controller
	/// \param robot the robot that uses the controller	/// \param robot the robot that uses the controller
	/// \param args extra arguments that the controller takes.	/// \param args extra arguments that the controller takes.
	/// \return true on successful initialization	/// \return true on successful initialization
	virtual bool Init(RobotBasePtr robot, const std::string& args) RAVE_DEP RECATED {	virtual bool Init(RobotBasePtr robot, const std::string& args) RAVE_DEP RECATED {
	if( !robot ) {	if( !robot ) {
	return false;	return false;
	}	}
	std::vector<int> dofindices;	std::vector<int> dofindices;

	skipping to change at line 88	skipping to change at line 92

	/// \brief Follow a path in configuration space, adds to the queue of t rajectories already in execution. <b>[multi-thread safe]</b>	/// \brief Follow a path in configuration space, adds to the queue of t rajectories already in execution. <b>[multi-thread safe]</b>
	/// \param ptraj - the trajectory	/// \param ptraj - the trajectory
	/// \return true if trajectory operation successful	/// \return true if trajectory operation successful
	virtual bool SetPath(TrajectoryBaseConstPtr ptraj) = 0;	virtual bool SetPath(TrajectoryBaseConstPtr ptraj) = 0;

	/// \brief Simulate one step forward for controllers running in the sim ulation environment	/// \brief Simulate one step forward for controllers running in the sim ulation environment
	/// \param fTimeElapsed - time elapsed in simulation environment since last frame	/// \param fTimeElapsed - time elapsed in simulation environment since last frame
	virtual void SimulationStep(dReal fTimeElapsed) = 0;	virtual void SimulationStep(dReal fTimeElapsed) = 0;


	/// \return true when goal reached. If a trajectory was set, return onl	/// \brief Return true when goal reached.
	y when	///
	/// trajectory is done. If SetDesired was called, return only w	/// If a trajectory was set, return only when
	hen robot is	/// trajectory is done. If SetDesired was called, return only when robo
	/// is at the desired location. If SendCommand sent, returns tr	t is
	ue when the command	/// is at the desired location. If SendCommand sent, returns true when
	/// was completed by the hand.	the command
		/// was completed by the hand.
	virtual bool IsDone() = 0;	virtual bool IsDone() = 0;


	/// \return the time along the current command	/// \brief return the time along the current command
	virtual dReal GetTime() const OPENRAVE_DUMMY_IMPLEMENTATION;	virtual dReal GetTime() const OPENRAVE_DUMMY_IMPLEMENTATION;

	/// \brief get velocity of the controlled DOFs	/// \brief get velocity of the controlled DOFs

		///
	/// \param vel [out] - current velocity of robot from the dof	/// \param vel [out] - current velocity of robot from the dof
	virtual void GetVelocity(std::vector<dReal>& vel) const OPENRAVE_DUMMY_ IMPLEMENTATION;	virtual void GetVelocity(std::vector<dReal>& vel) const OPENRAVE_DUMMY_ IMPLEMENTATION;

	/// get torque/current/strain values	/// get torque/current/strain values
	/// \param torque [out] - returns the current torque/current/strain exe rted by each of the dofs from outside forces.	/// \param torque [out] - returns the current torque/current/strain exe rted by each of the dofs from outside forces.
	/// The feedforward and friction terms should be subtracted out already	/// The feedforward and friction terms should be subtracted out already
	virtual void GetTorque(std::vector<dReal>& torque) const OPENRAVE_DUMMY _IMPLEMENTATION;	virtual void GetTorque(std::vector<dReal>& torque) const OPENRAVE_DUMMY _IMPLEMENTATION;


	// Specifies the controlled degrees of freedom used to control the r	// Specifies the controlled degrees of freedom used to control t
	obot through torque In the	he robot through torque In the
	// general sense, it is not always the case that there's a one-to-on	// general sense, it is not always the case that there's a one-t
	e mapping between a robot's	o-one mapping between a robot's
	// joints and the motors used to control the robot. A good example o	// joints and the motors used to control the robot. A good examp
	f this is a	le of this is a
	// differential-drive robot. If developers need such a robot, they s	// differential-drive robot. If developers need such a robot, th
	hould derive from RobotBase	ey should derive from RobotBase
	// and override these methods. The function that maps control torque	// and override these methods. The function that maps control to
	s to actual movements of	rques to actual movements of
	// the robot should be put in SimulationStep. As default, the contr	// the robot should be put in SimulationStep. As default, the c
	ol degrees of freedom are	ontrol degrees of freedom are
	// tied directly to the active degrees of freedom; the max torques f	// tied directly to the active degrees of freedom; the max torqu
	or affine dofs are 0 in	es for affine dofs are 0 in
	// this case.	// this case.
	// virtual void GetControlMaxTorques(std::vector<dReal>& vmaxtorque) con	// virtual void GetControlMaxTorques(std::vector<dReal>& vmaxtorque)
	st;	const;
	// virtual void SetControlTorques(const std::vector<dReal>& pTorques);	// virtual void SetControlTorques(const std::vector<dReal>& pTorques
		);

	private:	private:

	virtual const char* GetHash() const { return OPENRAVE_CONTROLLER_HASH;	virtual const char* GetHash() const {
	}	return OPENRAVE_CONTROLLER_HASH;
		}
	};	};

	/** \brief controller that manage multiple controllers, allows users to eas ily set multiple controllers for one robot.	/** \brief controller that manage multiple controllers, allows users to eas ily set multiple controllers for one robot.

	The class also make sure individual controllers do not have colliding D OF. It ignores the	The class also make sure individual controllers do not have colliding D OF. It ignores the
	*/	*/
	class OPENRAVE_API MultiController : public ControllerBase	class OPENRAVE_API MultiController : public ControllerBase
	{	{
	public:	public:
	MultiController(EnvironmentBasePtr penv);	MultiController(EnvironmentBasePtr penv);

	skipping to change at line 168	skipping to change at line 177
	/// \brief return the maximum time	/// \brief return the maximum time
	virtual dReal GetTime() const;	virtual dReal GetTime() const;

	virtual void GetVelocity(std::vector<dReal>& vel) const;	virtual void GetVelocity(std::vector<dReal>& vel) const;

	/// get torque/current/strain values	/// get torque/current/strain values
	/// \param torque [out] - returns the current torque/current/strain exe rted by each of the dofs from outside forces.	/// \param torque [out] - returns the current torque/current/strain exe rted by each of the dofs from outside forces.
	/// The feedforward and friction terms should be subtracted out already	/// The feedforward and friction terms should be subtracted out already
	virtual void GetTorque(std::vector<dReal>& torque) const;	virtual void GetTorque(std::vector<dReal>& torque) const;


	protected:	protected:
	RobotBasePtr _probot;	RobotBasePtr _probot;
	std::vector<int> _dofindices, _dofreverseindices;	std::vector<int> _dofindices, _dofreverseindices;
	int _nControlTransformation;	int _nControlTransformation;
	std::list<ControllerBasePtr> _listcontrollers;	std::list<ControllerBasePtr> _listcontrollers;
	std::vector<ControllerBasePtr> _vcontrollersbydofs;	std::vector<ControllerBasePtr> _vcontrollersbydofs;
	ControllerBasePtr _ptransformcontroller;	ControllerBasePtr _ptransformcontroller;
	TrajectoryBasePtr _ptraj;	TrajectoryBasePtr _ptraj;
	mutable boost::mutex _mutex;	mutable boost::mutex _mutex;
	};	};


End of changes. 11 change blocks.
	37 lines changed or deleted	44 lines changed or added

	geometry.h	geometry.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2011 Rosen Diankov (rosen.diankov@gmail.com)	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 49	skipping to change at line 49

	namespace OpenRAVE {	namespace OpenRAVE {

	/// Templated math and geometric functions	/// Templated math and geometric functions
	namespace geometry {	namespace geometry {

	template <typename T> class RaveTransform;	template <typename T> class RaveTransform;
	template <typename T> class RaveTransformMatrix;	template <typename T> class RaveTransformMatrix;

	#ifndef MATH_SQRT	#ifndef MATH_SQRT

	inline float MATH_SQRT(float f) { return sqrtf(f); }	inline float MATH_SQRT(float f) {
	inline double MATH_SQRT(double f) { return sqrt(f); }	return sqrtf(f);
		}
		inline double MATH_SQRT(double f) {
		return sqrt(f);
		}
	#endif	#endif
	#ifndef MATH_SIN	#ifndef MATH_SIN

	inline float MATH_SIN(float f) { return sinf(f); }	inline float MATH_SIN(float f) {
	inline double MATH_SIN(double f) { return sin(f); }	return sinf(f);
		}
		inline double MATH_SIN(double f) {
		return sin(f);
		}
	#endif	#endif
	#ifndef MATH_COS	#ifndef MATH_COS

	inline float MATH_COS(float f) { return cosf(f); }	inline float MATH_COS(float f) {
	inline double MATH_COS(double f) { return cos(f); }	return cosf(f);
		}
		inline double MATH_COS(double f) {
		return cos(f);
		}
	#endif	#endif
	#ifndef MATH_FABS	#ifndef MATH_FABS

	inline float MATH_FABS(float f) { return fabsf(f); }	inline float MATH_FABS(float f) {
	inline double MATH_FABS(double f) { return fabs(f); }	return fabsf(f);
		}
		inline double MATH_FABS(double f) {
		return fabs(f);
		}
	#endif	#endif
	#ifndef MATH_ACOS	#ifndef MATH_ACOS

	inline float MATH_ACOS(float f) { return acosf(f); }	inline float MATH_ACOS(float f) {
	inline double MATH_ACOS(double f) { return acos(f); }	return acosf(f);
		}
		inline double MATH_ACOS(double f) {
		return acos(f);
		}
	#endif	#endif
	#ifndef MATH_ASIN	#ifndef MATH_ASIN

	inline float MATH_ASIN(float f) { return asinf(f); }	inline float MATH_ASIN(float f) {
	inline double MATH_ASIN(double f) { return asin(f); }	return asinf(f);
		}
		inline double MATH_ASIN(double f) {
		return asin(f);
		}
	#endif	#endif
	#ifndef MATH_ATAN2	#ifndef MATH_ATAN2

	inline float MATH_ATAN2(float fy, float fx) { return atan2f(fy,fx); }	inline float MATH_ATAN2(float fy, float fx) {
	inline double MATH_ATAN2(double fy, double fx) { return atan2(fy,fx); }	return atan2f(fy,fx);
		}
		inline double MATH_ATAN2(double fy, double fx) {
		return atan2(fy,fx);
		}
	#endif	#endif

	/** \brief Vector class containing 4 dimensions.	/** \brief Vector class containing 4 dimensions.

	\ingroup affine_math	\ingroup affine_math
	It is better to use this for a 3 dim vector because it is 16byte align ed and SIMD instructions can be used	It is better to use this for a 3 dim vector because it is 16byte align ed and SIMD instructions can be used

	*/	*/
	template <typename T>	template <typename T>
	class RaveVector	class RaveVector
	{	{
	public:	public:
	T x, y, z, w;	T x, y, z, w;


	RaveVector() : x(0), y(0), z(0), w(0) {}	RaveVector() : x(0), y(0), z(0), w(0) {
		}


	RaveVector(T x, T y, T z) : x(x), y(y), z(z), w(0) {}	RaveVector(T x, T y, T z) : x(x), y(y), z(z), w(0) {
	RaveVector(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {}	}
	template<typename U> RaveVector(const RaveVector<U> &vec) : x((T)vec.x)	RaveVector(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {
	, y((T)vec.y), z((T)vec.z), w((T)vec.w) {}	}
		template<typename U> RaveVector(const RaveVector<U> &vec) : x((T)vec.x)
		, y((T)vec.y), z((T)vec.z), w((T)vec.w) {
		}

	/// note, it only copes 3 values!	/// note, it only copes 3 values!

	template<typename U> RaveVector(const U* pf) { MATH_ASSERT(pf != NULL);	template<typename U> RaveVector(const U* pf) {
	x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2]; w = 0; }	MATH_ASSERT(pf != NULL); x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2];
		w = 0;
		}


	T operator[](int i) const { return (&x)[i]; }	T operator[] (int i) const {
	T& operator[](int i) { return (&x)[i]; }	return (&x)[i];
		}
		T& operator[] (int i) {
		return (&x)[i];
		}

	// casting operators	// casting operators

	operator T* () { return &x; }	operator T* () {
		return &x;
		}
	operator const T* () const { return (const T*)&x; }	operator const T* () const { return (const T*)&x; }

	template <typename U>	template <typename U>

	RaveVector<T>& operator=(const RaveVector<U>& r) { x = (T)r.x; y = (T)r	RaveVector<T>& operator=(const RaveVector<U>&r) {
	.y; z = (T)r.z; w = (T)r.w; return *this; }	x = (T)r.x; y = (T)r.y; z = (T)r.z; w = (T)r.w; return *this;
		}

	// SCALAR FUNCTIONS	// SCALAR FUNCTIONS

	template <typename U> inline T dot(const RaveVector<U> &v) const { retu	template <typename U> inline T dot(const RaveVector<U> &v) const {
	rn xv.x + yv.y + zv.z + wv.w; }	return xv.x + yv.y + zv.z + wv.w;
	template <typename U> inline T dot3(const RaveVector<U> &v) const { ret	}
	urn xv.x + yv.y + z*v.z; }	template <typename U> inline T dot3(const RaveVector<U> &v) const {
	inline RaveVector<T>& normalize() { return normalize4(); }	return xv.x + yv.y + z*v.z;
		}
		inline RaveVector<T>& normalize() {
		return normalize4();
		}
	inline RaveVector<T>& normalize4() {	inline RaveVector<T>& normalize4() {
	T f = xx+yy+zz+ww;	T f = xx+yy+zz+ww;

	if( f < T(1)-std::numeric_limits<dReal>::epsilon() \|\| f > T(1)+std: :numeric_limits<dReal>::epsilon() ) {	if(( f < T(1)-std::numeric_limits<dReal>::epsilon()) \|\|( f > T(1)+s td::numeric_limits<dReal>::epsilon()) ) {
	MATH_ASSERT( f > 0 );	MATH_ASSERT( f > 0 );
	// yes it is faster to multiply by (1/f), but with 4 divides we gain precision (which is more important in robotics)	// yes it is faster to multiply by (1/f), but with 4 divides we gain precision (which is more important in robotics)
	f = MATH_SQRT(f);	f = MATH_SQRT(f);
	x /= f; y /= f; z /= f; w /= f;	x /= f; y /= f; z /= f; w /= f;
	}	}
	return *this;	return *this;
	}	}
	inline RaveVector<T>& normalize3() {	inline RaveVector<T>& normalize3() {
	T f = xx+yy+z*z;	T f = xx+yy+z*z;

	if( f < T(1)-std::numeric_limits<dReal>::epsilon() \|\| f > T(1)+std: :numeric_limits<dReal>::epsilon() ) {	if(( f < T(1)-std::numeric_limits<dReal>::epsilon()) \|\|( f > T(1)+s td::numeric_limits<dReal>::epsilon()) ) {
	MATH_ASSERT( f > 0 );	MATH_ASSERT( f > 0 );
	f = MATH_SQRT(f);	f = MATH_SQRT(f);
	x /= f; y /= f; z /= f;	x /= f; y /= f; z /= f;
	}	}
	return *this;	return *this;
	}	}


	inline T lengthsqr2() const { return xx + yy; }	inline T lengthsqr2() const {
	inline T lengthsqr3() const { return xx + yy + z*z; }	return xx + yy;
	inline T lengthsqr4() const { return xx + yy + zz + ww; }	}
		inline T lengthsqr3() const {
		return xx + yy + z*z;
		}
		inline T lengthsqr4() const {
		return xx + yy + zz + ww;
		}


	inline void Set3(const T* pvals) { x = pvals[0]; y = pvals[1]; z = pval	inline void Set3(const T* pvals) {
	s[2]; }	x = pvals[0]; y = pvals[1]; z = pvals[2];
	inline void Set3(T val1, T val2, T val3) { x = val1; y = val2; z = val3	}
	; }	inline void Set3(T val1, T val2, T val3) {
	inline void Set4(const T* pvals) { x = pvals[0]; y = pvals[1]; z = pval	x = val1; y = val2; z = val3;
	s[2]; w = pvals[3]; }	}
	inline void Set4(T val1, T val2, T val3, T val4) { x = val1; y = val2;	inline void Set4(const T* pvals) {
	z = val3; w = val4;}	x = pvals[0]; y = pvals[1]; z = pvals[2]; w = pvals[3];
		}
		inline void Set4(T val1, T val2, T val3, T val4) {
		x = val1; y = val2; z = val3; w = val4;
		}
	/// 3 dim cross product, w is not touched	/// 3 dim cross product, w is not touched
	inline RaveVector<T> cross(const RaveVector<T> &v) const {	inline RaveVector<T> cross(const RaveVector<T> &v) const {
	RaveVector<T> ucrossv;	RaveVector<T> ucrossv;
	ucrossv[0] = y * v[2] - z * v[1];	ucrossv[0] = y * v[2] - z * v[1];
	ucrossv[1] = z * v[0] - x * v[2];	ucrossv[1] = z * v[0] - x * v[2];
	ucrossv[2] = x * v[1] - y * v[0];	ucrossv[2] = x * v[1] - y * v[0];
	return ucrossv;	return ucrossv;
	}	}


	inline RaveVector<T>& Cross(const RaveVector<T> &v) RAVE_DEPRECATED { C	inline RaveVector<T>& Cross(const RaveVector<T> &v) RAVE_DEPRECATED {
	ross(this, v); return this; }	Cross(this, v); return this;
		}
	inline RaveVector<T>& Cross(const RaveVector<T> &u, const RaveVector<T> &v) RAVE_DEPRECATED	inline RaveVector<T>& Cross(const RaveVector<T> &u, const RaveVector<T> &v) RAVE_DEPRECATED
	{	{
	RaveVector<T> ucrossv;	RaveVector<T> ucrossv;
	ucrossv[0] = u[1] * v[2] - u[2] * v[1];	ucrossv[0] = u[1] * v[2] - u[2] * v[1];
	ucrossv[1] = u[2] * v[0] - u[0] * v[2];	ucrossv[1] = u[2] * v[0] - u[0] * v[2];
	ucrossv[2] = u[0] * v[1] - u[1] * v[0];	ucrossv[2] = u[0] * v[1] - u[1] * v[0];
	*this = ucrossv;	*this = ucrossv;
	return *this;	return *this;
	}	}


	inline RaveVector<T> operator-() const { RaveVector<T> v; v.x = -x; v.y	inline RaveVector<T> operator-() const {
	= -y; v.z = -z; v.w = -w; return v; }	RaveVector<T> v; v.x = -x; v.y = -y; v.z = -z; v.w = -w; return v;
	template <typename U> inline RaveVector<T> operator+(const RaveVector<U	}
	> &r) const { RaveVector<T> v; v.x = x+r.x; v.y = y+r.y; v.z = z+r.z; v.w =	template <typename U> inline RaveVector<T> operator+(const RaveVector<U
	w+r.w; return v; }	> &r) const {
	template <typename U> inline RaveVector<T> operator-(const RaveVector<U	RaveVector<T> v; v.x = x+r.x; v.y = y+r.y; v.z = z+r.z; v.w = w+r.w
	> &r) const { RaveVector<T> v; v.x = x-r.x; v.y = y-r.y; v.z = z-r.z; v.w =	; return v;
	w-r.w; return v; }	}
	template <typename U> inline RaveVector<T> operator*(const RaveVector<U	template <typename U> inline RaveVector<T> operator-(const RaveVector<U
	> &r) const { RaveVector<T> v; v.x = r.xx; v.y = r.yy; v.z = r.z*z; v.w =	> &r) const {
	r.w*w; return v; }	RaveVector<T> v; v.x = x-r.x; v.y = y-r.y; v.z = z-r.z; v.w = w-r.w
	inline RaveVector<T> operator(T k) const { RaveVector<T> v; v.x = kx;	; return v;
	v.y = ky; v.z = kz; v.w = k*w; return v; }	}
		template <typename U> inline RaveVector<T> operator*(const RaveVector<U
		> &r) const {
		RaveVector<T> v; v.x = r.xx; v.y = r.yy; v.z = r.zz; v.w = r.ww
		; return v;
		}
		inline RaveVector<T> operator*(T k) const {
		RaveVector<T> v; v.x = kx; v.y = ky; v.z = kz; v.w = kw; return
		v;
		}


	template <typename U> inline RaveVector<T>& operator += (const RaveVect	template <typename U> inline RaveVector<T>& operator += (const RaveVect
	or<U>& r) { x += r.x; y += r.y; z += r.z; w += r.w; return *this; }	or<U>&r) {
	template <typename U> inline RaveVector<T>& operator -= (const RaveVect	x += r.x; y += r.y; z += r.z; w += r.w; return *this;
	or<U>& r) { x -= r.x; y -= r.y; z -= r.z; w -= r.w; return *this; }	}
	template <typename U> inline RaveVector<T>& operator *= (const RaveVect	template <typename U> inline RaveVector<T>& operator -= (const RaveVect
	or<U>& r) { x = r.x; y = r.y; z = r.z; w = r.w; return *this; }	or<U>&r) {
		x -= r.x; y -= r.y; z -= r.z; w -= r.w; return *this;
		}
		template <typename U> inline RaveVector<T>& operator *= (const RaveVect
		or<U>&r) {
		x = r.x; y = r.y; z = r.z; w = r.w; return *this;
		}


	inline RaveVector<T>& operator = (const T k) { x = k; y = k; z = k;	inline RaveVector<T>& operator *= (const T k) {
	w = k; return this; }	x = k; y = k; z = k; w = k; return *this;
	inline RaveVector<T>& operator /= (const T _k) { T k=1/_k; x = k; y =	}
	k; z = k; w = k; return *this; }	inline RaveVector<T>& operator /= (const T _k) {
		T k=1/_k; x = k; y = k; z = k; w = k; return *this;
		}


	template <typename U> friend RaveVector<U> operator* (float f, const Ra	template <typename U> friend RaveVector<U> operator* (float f, const Ra
	veVector<U>& v);	veVector<U>&v);
	template <typename U> friend RaveVector<U> operator* (double f, const R	template <typename U> friend RaveVector<U> operator* (double f, const R
	aveVector<U>& v);	aveVector<U>&v);


	template <typename U> friend std::ostream& operator<<(std::ostream& O,	template <typename U> friend std::ostream& operator<<(std::ostream& O,
	const RaveVector<U>& v);	const RaveVector<U>&v);
	template <typename U> friend std::istream& operator>>(std::istream& I,	template <typename U> friend std::istream& operator>>(std::istream& I,
	RaveVector<U>& v);	RaveVector<U>&v);

	/// cross product operator	/// cross product operator
	template <typename U> inline RaveVector<T> operator^(const RaveVector<U > &v) const {	template <typename U> inline RaveVector<T> operator^(const RaveVector<U > &v) const {
	RaveVector<T> ucrossv;	RaveVector<T> ucrossv;
	ucrossv[0] = y * v[2] - z * v[1];	ucrossv[0] = y * v[2] - z * v[1];
	ucrossv[1] = z * v[0] - x * v[2];	ucrossv[1] = z * v[0] - x * v[2];
	ucrossv[2] = x * v[1] - y * v[0];	ucrossv[2] = x * v[1] - y * v[0];
	return ucrossv;	return ucrossv;
	}	}
	};	};

	template <typename T>	template <typename T>

	inline RaveVector<T> operator* (float f, const RaveVector<T>& left)	inline RaveVector<T> operator* (float f, const RaveVector<T>&left)
	{	{
	RaveVector<T> v;	RaveVector<T> v;
	v.x = (T)f * left.x;	v.x = (T)f * left.x;
	v.y = (T)f * left.y;	v.y = (T)f * left.y;
	v.z = (T)f * left.z;	v.z = (T)f * left.z;
	v.w = (T)f * left.w;	v.w = (T)f * left.w;
	return v;	return v;
	}	}

	template <typename T>	template <typename T>

	inline RaveVector<T> operator* (double f, const RaveVector<T>& left)	inline RaveVector<T> operator* (double f, const RaveVector<T>&left)
	{	{
	RaveVector<T> v;	RaveVector<T> v;
	v.x = (T)f * left.x;	v.x = (T)f * left.x;
	v.y = (T)f * left.y;	v.y = (T)f * left.y;
	v.z = (T)f * left.z;	v.z = (T)f * left.z;
	v.w = (T)f * left.w;	v.w = (T)f * left.w;
	return v;	return v;
	}	}

	/** \brief Affine transformation parameterized with quaterions.	/** \brief Affine transformation parameterized with quaterions.

	\ingroup affine_math	\ingroup affine_math

	*/	*/
	template <typename T>	template <typename T>
	class RaveTransform	class RaveTransform
	{	{
	public:	public:

	RaveTransform() { rot.x = 1; }	RaveTransform() {
		rot.x = 1;
		}
	template <typename U> RaveTransform(const RaveTransform<U>& t) {	template <typename U> RaveTransform(const RaveTransform<U>& t) {
	rot = t.rot;	rot = t.rot;
	trans = t.trans;	trans = t.trans;
	T fnorm = rot.lengthsqr4();	T fnorm = rot.lengthsqr4();
	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
	}	}
	template <typename U> RaveTransform(const RaveVector<U>& rot, const Rav eVector<U>& trans) : rot(rot), trans(trans) {	template <typename U> RaveTransform(const RaveVector<U>& rot, const Rav eVector<U>& trans) : rot(rot), trans(trans) {
	T fnorm = rot.lengthsqr4();	T fnorm = rot.lengthsqr4();
	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
	}	}

	inline RaveTransform(const RaveTransformMatrix<T>& t);	inline RaveTransform(const RaveTransformMatrix<T>&t);

	void identity() {	void identity() {
	rot.x = 1; rot.y = rot.z = rot.w = 0;	rot.x = 1; rot.y = rot.z = rot.w = 0;
	trans.x = trans.y = trans.z = 0;	trans.x = trans.y = trans.z = 0;
	}	}

	/// transform a 3 dim vector	/// transform a 3 dim vector

	inline RaveVector<T> operator* (const RaveVector<T>& r) const {	inline RaveVector<T> operator* (const RaveVector<T>&r) const {
	return trans + rotate(r);	return trans + rotate(r);
	}	}

	/// transform a vector by the rotation component only	/// transform a vector by the rotation component only
	inline RaveVector<T> rotate(const RaveVector<T>& r) const {	inline RaveVector<T> rotate(const RaveVector<T>& r) const {
	T xx = 2 * rot.y * rot.y;	T xx = 2 * rot.y * rot.y;
	T xy = 2 * rot.y * rot.z;	T xy = 2 * rot.y * rot.z;
	T xz = 2 * rot.y * rot.w;	T xz = 2 * rot.y * rot.w;
	T xw = 2 * rot.y * rot.x;	T xw = 2 * rot.y * rot.x;
	T yy = 2 * rot.z * rot.z;	T yy = 2 * rot.z * rot.z;

	skipping to change at line 276	skipping to change at line 362
	t.rot.z = rot.xr.rot.z + rot.zr.rot.x + rot.wr.rot.y - rot.yr.r ot.w;	t.rot.z = rot.xr.rot.z + rot.zr.rot.x + rot.wr.rot.y - rot.yr.r ot.w;
	t.rot.w = rot.xr.rot.w + rot.wr.rot.x + rot.yr.rot.z - rot.zr.r ot.y;	t.rot.w = rot.xr.rot.w + rot.wr.rot.x + rot.yr.rot.z - rot.zr.r ot.y;
	// normalize the transformation	// normalize the transformation
	T fnorm = t.rot.lengthsqr4();	T fnorm = t.rot.lengthsqr4();
	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
	t.rot.normalize4();	t.rot.normalize4();
	return t;	return t;
	}	}

	/// t = this * r	/// t = this * r

	inline RaveTransform<T> operator* (const RaveTransform<T>& r) const {	inline RaveTransform<T> operator* (const RaveTransform<T>&r) const {
	RaveTransform<T> t;	RaveTransform<T> t;
	t.trans = operator*(r.trans);	t.trans = operator*(r.trans);
	t.rot.x = rot.xr.rot.x - rot.yr.rot.y - rot.zr.rot.z - rot.wr.r ot.w;	t.rot.x = rot.xr.rot.x - rot.yr.rot.y - rot.zr.rot.z - rot.wr.r ot.w;
	t.rot.y = rot.xr.rot.y + rot.yr.rot.x + rot.zr.rot.w - rot.wr.r ot.z;	t.rot.y = rot.xr.rot.y + rot.yr.rot.x + rot.zr.rot.w - rot.wr.r ot.z;
	t.rot.z = rot.xr.rot.z + rot.zr.rot.x + rot.wr.rot.y - rot.yr.r ot.w;	t.rot.z = rot.xr.rot.z + rot.zr.rot.x + rot.wr.rot.y - rot.yr.r ot.w;
	t.rot.w = rot.xr.rot.w + rot.wr.rot.x + rot.yr.rot.z - rot.zr.r ot.y;	t.rot.w = rot.xr.rot.w + rot.wr.rot.x + rot.yr.rot.z - rot.zr.r ot.y;
	// normalize the transformation	// normalize the transformation
	T fnorm = t.rot.lengthsqr4();	T fnorm = t.rot.lengthsqr4();
	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
	t.rot.normalize4();	t.rot.normalize4();
	return t;	return t;
	}	}


	inline RaveTransform<T>& operator*= (const RaveTransform<T>& right) {	inline RaveTransform<T>& operator*= (const RaveTransform<T>&right) {
	this = operator(right);	this = operator(right);
	return *this;	return *this;
	}	}

	inline RaveTransform<T> inverse() const {	inline RaveTransform<T> inverse() const {
	RaveTransform<T> inv;	RaveTransform<T> inv;
	inv.rot.x = rot.x;	inv.rot.x = rot.x;
	inv.rot.y = -rot.y;	inv.rot.y = -rot.y;
	inv.rot.z = -rot.z;	inv.rot.z = -rot.z;
	inv.rot.w = -rot.w;	inv.rot.w = -rot.w;

	inv.trans = -inv.rotate(trans);	inv.trans = -inv.rotate(trans);
	return inv;	return inv;
	}	}


	template <typename U> inline RaveTransform<T>& operator= (const RaveTra nsform<U>& r) {	template <typename U> inline RaveTransform<T>& operator= (const RaveTra nsform<U>&r) {
	trans = r.trans;	trans = r.trans;
	rot = r.rot;	rot = r.rot;
	T fnorm = rot.lengthsqr4();	T fnorm = rot.lengthsqr4();
	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );	MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
	return *this;	return *this;
	}	}


	template <typename U> friend std::ostream& operator<<(std::ostream& O,	template <typename U> friend std::ostream& operator<<(std::ostream& O,
	const RaveTransform<U>& v);	const RaveTransform<U>&v);
	template <typename U> friend std::istream& operator>>(std::istream& I,	template <typename U> friend std::istream& operator>>(std::istream& I,
	RaveTransform<U>& v);	RaveTransform<U>&v);


	RaveVector<T> rot, trans; ///< rot is a quaternion=(cos(ang/2),axisxsi n(ang/2),axisysin(ang/2),axisz*sin(ang/2))	RaveVector<T> rot, trans; ///< rot is a quaternion=(cos(ang/2),axis xsin(ang/2),axisysin(ang/2),axisz*sin(ang/2))
	};	};

	/** \brief Affine transformation parameterized with rotation matrices. Scal es and shears are not supported.	/** \brief Affine transformation parameterized with rotation matrices. Scal es and shears are not supported.

	\ingroup affine_math	\ingroup affine_math

	*/	*/
	template <typename T>	template <typename T>
	class RaveTransformMatrix	class RaveTransformMatrix
	{	{
	public:	public:

	inline RaveTransformMatrix() { identity(); m[3] = m[7] = m[11] = 0; }	inline RaveTransformMatrix() {
		identity(); m[3] = m[7] = m[11] = 0;
		}
	template <typename U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) {	template <typename U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) {
	// don't memcpy!	// don't memcpy!
	m[0] = t.m[0]; m[1] = t.m[1]; m[2] = t.m[2]; m[3] = t.m[3];	m[0] = t.m[0]; m[1] = t.m[1]; m[2] = t.m[2]; m[3] = t.m[3];
	m[4] = t.m[4]; m[5] = t.m[5]; m[6] = t.m[6]; m[7] = t.m[7];	m[4] = t.m[4]; m[5] = t.m[5]; m[6] = t.m[6]; m[7] = t.m[7];
	m[8] = t.m[8]; m[9] = t.m[9]; m[10] = t.m[10]; m[11] = t.m[11];	m[8] = t.m[8]; m[9] = t.m[9]; m[10] = t.m[10]; m[11] = t.m[11];
	trans = t.trans;	trans = t.trans;
	}	}

	inline RaveTransformMatrix(const RaveTransform<T>& t);	inline RaveTransformMatrix(const RaveTransform<T>&t);

	inline void identity() {	inline void identity() {
	m[0] = 1; m[1] = 0; m[2] = 0;	m[0] = 1; m[1] = 0; m[2] = 0;
	m[4] = 0; m[5] = 1; m[6] = 0;	m[4] = 0; m[5] = 1; m[6] = 0;
	m[8] = 0; m[9] = 0; m[10] = 1;	m[8] = 0; m[9] = 0; m[10] = 1;
	trans.x = trans.y = trans.z = 0;	trans.x = trans.y = trans.z = 0;
	}	}

	inline void rotfrommat(T m_00, T m_01, T m_02, T m_10, T m_11, T m_12, T m_20, T m_21, T m_22) {	inline void rotfrommat(T m_00, T m_01, T m_02, T m_10, T m_11, T m_12, T m_20, T m_21, T m_22) {
	m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0;	m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0;

	skipping to change at line 361	skipping to change at line 449
	inline T rot(int i, int j) const {	inline T rot(int i, int j) const {
	MATH_ASSERT( i >= 0 && i < 3 && j >= 0 && j < 3);	MATH_ASSERT( i >= 0 && i < 3 && j >= 0 && j < 3);
	return m[4*i+j];	return m[4*i+j];
	}	}
	inline T& rot(int i, int j) {	inline T& rot(int i, int j) {
	MATH_ASSERT( i >= 0 && i < 3 && j >= 0 && j < 3);	MATH_ASSERT( i >= 0 && i < 3 && j >= 0 && j < 3);
	return m[4*i+j];	return m[4*i+j];
	}	}

	template <typename U>	template <typename U>

	inline RaveVector<T> operator* (const RaveVector<U>& r) const {	inline RaveVector<T> operator* (const RaveVector<U>&r) const {
	RaveVector<T> v;	RaveVector<T> v;
	v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x;	v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x;
	v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y;	v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y;
	v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z;	v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z;
	return v;	return v;
	}	}

	/// t = this * r	/// t = this * r

	inline RaveTransformMatrix<T> operator* (const RaveTransformMatrix<T>& r) const {	inline RaveTransformMatrix<T> operator* (const RaveTransformMatrix<T>&r ) const {
	RaveTransformMatrix<T> t;	RaveTransformMatrix<T> t;
	t.m[04+0] = m[04+0]r.m[04+0]+m[04+1]r.m[14+0]+m[04+2]r.m[2 4+0];	t.m[04+0] = m[04+0]r.m[04+0]+m[04+1]r.m[14+0]+m[04+2]r.m[2 4+0];
	t.m[04+1] = m[04+0]r.m[04+1]+m[04+1]r.m[14+1]+m[04+2]r.m[2 4+1];	t.m[04+1] = m[04+0]r.m[04+1]+m[04+1]r.m[14+1]+m[04+2]r.m[2 4+1];
	t.m[04+2] = m[04+0]r.m[04+2]+m[04+1]r.m[14+2]+m[04+2]r.m[2 4+2];	t.m[04+2] = m[04+0]r.m[04+2]+m[04+1]r.m[14+2]+m[04+2]r.m[2 4+2];
	t.m[14+0] = m[14+0]r.m[04+0]+m[14+1]r.m[14+0]+m[14+2]r.m[2 4+0];	t.m[14+0] = m[14+0]r.m[04+0]+m[14+1]r.m[14+0]+m[14+2]r.m[2 4+0];
	t.m[14+1] = m[14+0]r.m[04+1]+m[14+1]r.m[14+1]+m[14+2]r.m[2 4+1];	t.m[14+1] = m[14+0]r.m[04+1]+m[14+1]r.m[14+1]+m[14+2]r.m[2 4+1];
	t.m[14+2] = m[14+0]r.m[04+2]+m[14+1]r.m[14+2]+m[14+2]r.m[2 4+2];	t.m[14+2] = m[14+0]r.m[04+2]+m[14+1]r.m[14+2]+m[14+2]r.m[2 4+2];
	t.m[24+0] = m[24+0]r.m[04+0]+m[24+1]r.m[14+0]+m[24+2]r.m[2 4+0];	t.m[24+0] = m[24+0]r.m[04+0]+m[24+1]r.m[14+0]+m[24+2]r.m[2 4+0];
	t.m[24+1] = m[24+0]r.m[04+1]+m[24+1]r.m[14+1]+m[24+2]r.m[2 4+1];	t.m[24+1] = m[24+0]r.m[04+1]+m[24+1]r.m[14+1]+m[24+2]r.m[2 4+1];
	t.m[24+2] = m[24+0]r.m[04+2]+m[24+1]r.m[14+2]+m[24+2]r.m[2 4+2];	t.m[24+2] = m[24+0]r.m[04+2]+m[24+1]r.m[14+2]+m[24+2]r.m[2 4+2];
	t.trans[0] = r.trans[0] * m[0] + r.trans[1] * m[1] + r.trans[2] * m [2] + trans.x;	t.trans[0] = r.trans[0] * m[0] + r.trans[1] * m[1] + r.trans[2] * m [2] + trans.x;
	t.trans[1] = r.trans[0] * m[4] + r.trans[1] * m[5] + r.trans[2] * m [6] + trans.y;	t.trans[1] = r.trans[0] * m[4] + r.trans[1] * m[5] + r.trans[2] * m [6] + trans.y;
	t.trans[2] = r.trans[0] * m[8] + r.trans[1] * m[9] + r.trans[2] * m [10] + trans.z;	t.trans[2] = r.trans[0] * m[8] + r.trans[1] * m[9] + r.trans[2] * m [10] + trans.z;
	return t;	return t;
	}	}


	inline RaveTransformMatrix<T> operator*= (const RaveTransformMatrix<T>& r) const {	inline RaveTransformMatrix<T> operator*= (const RaveTransformMatrix<T>& r) const {
	this = this * r;	this = this * r;
	return *this;	return *this;
	}	}

	template <typename U>	template <typename U>
	inline RaveVector<U> rotate(const RaveVector<U>& r) const {	inline RaveVector<U> rotate(const RaveVector<U>& r) const {
	RaveVector<U> v;	RaveVector<U> v;
	v.x = r.x * m[0] + r.y * m[1] + r.z * m[2];	v.x = r.x * m[0] + r.y * m[1] + r.z * m[2];
	v.y = r.x * m[4] + r.y * m[5] + r.z * m[6];	v.y = r.x * m[4] + r.y * m[5] + r.z * m[6];
	v.z = r.x * m[8] + r.y * m[9] + r.z * m[10];	v.z = r.x * m[8] + r.y * m[9] + r.z * m[10];

	skipping to change at line 435	skipping to change at line 523
	inv.m[04+2] = m[04 + 1] * m[14 + 2] - m[04 + 2] * m[1*4 + 1];	inv.m[04+2] = m[04 + 1] * m[14 + 2] - m[04 + 2] * m[1*4 + 1];
	inv.m[14+0] = m[14 + 2] * m[24 + 0] - m[14 + 0] * m[2*4 + 2];	inv.m[14+0] = m[14 + 2] * m[24 + 0] - m[14 + 0] * m[2*4 + 2];
	inv.m[14+1] = m[04 + 0] * m[24 + 2] - m[04 + 2] * m[2*4 + 0];	inv.m[14+1] = m[04 + 0] * m[24 + 2] - m[04 + 2] * m[2*4 + 0];
	inv.m[14+2] = m[04 + 2] * m[14 + 0] - m[04 + 0] * m[1*4 + 2];	inv.m[14+2] = m[04 + 2] * m[14 + 0] - m[04 + 0] * m[1*4 + 2];
	inv.m[24+0] = m[14 + 0] * m[24 + 1] - m[14 + 1] * m[2*4 + 0];	inv.m[24+0] = m[14 + 0] * m[24 + 1] - m[14 + 1] * m[2*4 + 0];
	inv.m[24+1] = m[04 + 1] * m[24 + 0] - m[04 + 0] * m[2*4 + 1];	inv.m[24+1] = m[04 + 1] * m[24 + 0] - m[04 + 0] * m[2*4 + 1];
	inv.m[24+2] = m[04 + 0] * m[14 + 1] - m[04 + 1] * m[1*4 + 0];	inv.m[24+2] = m[04 + 0] * m[14 + 1] - m[04 + 1] * m[1*4 + 0];
	T fdet = m[04 + 2] inv.m[24+0] + m[14 + 2] * inv.m[24+1] + m[ 24 + 2] * inv.m[2*4+2];	T fdet = m[04 + 2] inv.m[24+0] + m[14 + 2] * inv.m[24+1] + m[ 24 + 2] * inv.m[2*4+2];
	MATH_ASSERT(fdet>=0);	MATH_ASSERT(fdet>=0);
	fdet = 1 / fdet;	fdet = 1 / fdet;

	inv.m[04+0] = fdet; inv.m[04+1] = fdet;	inv.m[04+0] = fdet; inv.m[04+1] = fdet; inv
	inv.m[04+2] = fdet;	.m[04+2] = fdet;
	inv.m[14+0] = fdet; inv.m[14+1] = fdet;	inv.m[14+0] = fdet; inv.m[14+1] = fdet; inv
	inv.m[14+2] = fdet;	.m[14+2] = fdet;
	inv.m[24+0] = fdet; inv.m[24+1] = fdet;	inv.m[24+0] = fdet; inv.m[24+1] = fdet; inv
	inv.m[24+2] = fdet;	.m[24+2] = fdet;
	inv.trans = -inv.rotate(trans);	inv.trans = -inv.rotate(trans);
	return inv;	return inv;
	}	}

	template <typename U>	template <typename U>
	inline void Extract(RaveVector<U>& right, RaveVector<U>& up, RaveVector <U>& dir, RaveVector<U>& pos) const {	inline void Extract(RaveVector<U>& right, RaveVector<U>& up, RaveVector <U>& dir, RaveVector<U>& pos) const {
	pos = trans;	pos = trans;
	right.x = m[0]; up.x = m[1]; dir.x = m[2];	right.x = m[0]; up.x = m[1]; dir.x = m[2];
	right.y = m[4]; up.y = m[5]; dir.y = m[6];	right.y = m[4]; up.y = m[5]; dir.y = m[6];
	right.z = m[8]; up.z = m[9]; dir.z = m[10];	right.z = m[8]; up.z = m[9]; dir.z = m[10];
	}	}


	template <typename U> friend std::ostream& operator<<(std::ostream& O,	template <typename U> friend std::ostream& operator<<(std::ostream& O,
	const RaveTransformMatrix<U>& v);	const RaveTransformMatrix<U>&v);
	template <typename U> friend std::istream& operator>>(std::istream& I,	template <typename U> friend std::istream& operator>>(std::istream& I,
	RaveTransformMatrix<U>& v);	RaveTransformMatrix<U>&v);

	/// 3x3 rotation matrix. Note that each row is 4 elements long! So row 1 starts at m[4], row 2 at m[8]	/// 3x3 rotation matrix. Note that each row is 4 elements long! So row 1 starts at m[4], row 2 at m[8]
	/// The reason is to maintain 16 byte alignment when sizeof(T) is 4 byt es	/// The reason is to maintain 16 byte alignment when sizeof(T) is 4 byt es
	T m[12];	T m[12];

	RaveVector<T> trans; ///< translation component	RaveVector<T> trans; ///< translation component
	};	};

	/// \brief A ray defined by an origin and a direction.	/// \brief A ray defined by an origin and a direction.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	class ray	class ray
	{	{
	public:	public:

	ray() {}	ray() {
	ray(const RaveVector<T>& _pos, const RaveVector<T>& _dir) : pos(_pos),	}
	dir(_dir) {}	ray(const RaveVector<T>&_pos, const RaveVector<T>&_dir) : pos(_pos), di
		r(_dir) {
		}
	RaveVector<T> pos, dir;	RaveVector<T> pos, dir;
	};	};

	/// \brief An axis aligned bounding box.	/// \brief An axis aligned bounding box.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	class aabb	class aabb
	{	{
	public:	public:

	aabb() {}	aabb() {
	aabb(const RaveVector<T>& vpos, const RaveVector<T>& vextents) : pos(vp	}
	os), extents(vextents) {}	aabb(const RaveVector<T>&vpos, const RaveVector<T>&vextents) : pos(vpos
		), extents(vextents) {
		}
	RaveVector<T> pos, extents;	RaveVector<T> pos, extents;
	};	};

	/// \brief An oriented bounding box.	/// \brief An oriented bounding box.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	class obb	class obb
	{	{
	public:	public:
	RaveVector<T> right, up, dir, pos, extents;	RaveVector<T> right, up, dir, pos, extents;
	};	};

	/// \brief A triangle defined by 3 points.	/// \brief A triangle defined by 3 points.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	class triangle	class triangle
	{	{
	public:	public:

	triangle() {}	triangle() {
	triangle(const RaveVector<T>& v1, const RaveVector<T>& v2, const RaveVe	}
	ctor<T>& v3) : v1(v1), v2(v2), v3(v3) {}	triangle(const RaveVector<T>&v1, const RaveVector<T>&v2, const RaveVect
	~triangle() {}	or<T>&v3) : v1(v1), v2(v2), v3(v3) {
		}
		~triangle() {
		}


	RaveVector<T> v1, v2, v3; //!< the vertices of the triangle	RaveVector<T> v1, v2, v3; //!< the vertices of the triangle


	const RaveVector<T>& operator[](int i) const { return (&v1)[i]; }	const RaveVector<T>& operator[] (int i) const {
	RaveVector<T>& operator[](int i) { return (&v1)[i]; }	return (&v1)[i];
		}
		RaveVector<T>& operator[] (int i) {
		return (&v1)[i];
		}

	/// assumes CCW ordering of vertices	/// assumes CCW ordering of vertices
	inline RaveVector<T> normal() {	inline RaveVector<T> normal() {
	return (v2-v1).cross(v3-v1);	return (v2-v1).cross(v3-v1);
	}	}
	};	};

	/// \brief A pyramid with its vertex clipped.	/// \brief A pyramid with its vertex clipped.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>

	skipping to change at line 528	skipping to change at line 627
	T fnear, ffar;	T fnear, ffar;
	T ffovx,ffovy;	T ffovx,ffovy;
	T fcosfovx,fsinfovx,fcosfovy,fsinfovy;	T fcosfovx,fsinfovx,fcosfovy,fsinfovy;
	};	};

	/// \brief intrinsic parameters for a camera.	/// \brief intrinsic parameters for a camera.
	template <typename T>	template <typename T>
	class RaveCameraIntrinsics	class RaveCameraIntrinsics
	{	{
	public:	public:

	RaveCameraIntrinsics() : fx(0),fy(0),cx(0),cy(0), focal_length(0.01) {}	RaveCameraIntrinsics() : fx(0),fy(0),cx(0),cy(0), focal_length(0.01) {
	RaveCameraIntrinsics(T fx, T fy, T cx, T cy) : fx(fx), fy(fy), cx(cx),	}
	cy(cy), focal_length(0.01) {}	RaveCameraIntrinsics(T fx, T fy, T cx, T cy) : fx(fx), fy(fy), cx(cx),
		cy(cy), focal_length(0.01) {
		}

	template <typename U>	template <typename U>

	RaveCameraIntrinsics<T>& operator=(const RaveCameraIntrinsics<U>& r)	RaveCameraIntrinsics<T>& operator=(const RaveCameraIntrinsics<U>&r)
	{	{
	distortion_model = r.distortion_model;	distortion_model = r.distortion_model;
	distortion_coeffs.resize(r.distortion_coeffs.size());	distortion_coeffs.resize(r.distortion_coeffs.size());
	std::copy(r.distortion_coeffs.begin(),r.distortion_coeffs.end(),dis tortion_coeffs.begin());	std::copy(r.distortion_coeffs.begin(),r.distortion_coeffs.end(),dis tortion_coeffs.begin());
	focal_length = r.focal_length;	focal_length = r.focal_length;
	fx = r.fx;	fx = r.fx;
	fy = r.fy;	fy = r.fy;
	cx = r.cx;	cx = r.cx;
	cy = r.cy;	cy = r.cy;
	}	}

	T fx,fy, cx,cy;	T fx,fy, cx,cy;

	/** \brief distortion model of the camera. if left empty, no distortion model is used.	/** \brief distortion model of the camera. if left empty, no distortion model is used.

	Possible values are:	Possible values are:
	- "plumb_bob" - Brown. "Decentering Distortion of Lenses", Photomet ric Engineering, pages 444-462, Vol. 32, No. 3, 1966	- "plumb_bob" - Brown. "Decentering Distortion of Lenses", Photomet ric Engineering, pages 444-462, Vol. 32, No. 3, 1966

	*/	*/
	std::string distortion_model;	std::string distortion_model;

	std::vector<T> distortion_coeffs; ///< coefficients of the distortion m	std::vector<T> distortion_coeffs; ///< coefficients of the distorti
	odel	on model
	T focal_length; ///< physical focal length distance since focal length	T focal_length; ///< physical focal length distance since focal len
	cannot be recovered from the intrinsic matrix, but is necessary for determi	gth cannot be recovered from the intrinsic matrix, but is necessary for det
	ning the lens plane.	ermining the lens plane.
	};	};

	/// Don't add new lines to the output << operators. Some applications use i t to serialize the data	/// Don't add new lines to the output << operators. Some applications use i t to serialize the data
	/// to send across the network.	/// to send across the network.
	/// \name Primitive Serialization functions.	/// \name Primitive Serialization functions.
	//@{	//@{
	template <typename U>	template <typename U>

	std::ostream& operator<<(std::ostream& O, const RaveVector<U>& v)	std::ostream& operator<<(std::ostream& O, const RaveVector<U>&v)
	{	{
	return O << v.x << " " << v.y << " " << v.z << " " << v.w << " ";	return O << v.x << " " << v.y << " " << v.z << " " << v.w << " ";
	}	}

	template <typename U>	template <typename U>

	std::istream& operator>>(std::istream& I, RaveVector<U>& v)	std::istream& operator>>(std::istream& I, RaveVector<U>&v)
	{	{
	return I >> v.x >> v.y >> v.z >> v.w;	return I >> v.x >> v.y >> v.z >> v.w;
	}	}

	template <typename U>	template <typename U>

	std::ostream& operator<<(std::ostream& O, const RaveTransform<U>& v)	std::ostream& operator<<(std::ostream& O, const RaveTransform<U>&v)
	{	{
	return O << v.rot.x << " " << v.rot.y << " " << v.rot.z << " " << v.rot .w << " " << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";	return O << v.rot.x << " " << v.rot.y << " " << v.rot.z << " " << v.rot .w << " " << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
	}	}

	template <typename U>	template <typename U>

	std::istream& operator>>(std::istream& I, RaveTransform<U>& v)	std::istream& operator>>(std::istream& I, RaveTransform<U>&v)
	{	{
	return I >> v.rot.x >> v.rot.y >> v.rot.z >> v.rot.w >> v.trans.x >> v. trans.y >> v.trans.z;	return I >> v.rot.x >> v.rot.y >> v.rot.z >> v.rot.w >> v.trans.x >> v. trans.y >> v.trans.z;
	}	}

	template <typename U>	template <typename U>

	std::ostream& operator<<(std::ostream& O, const ray<U>& r)	std::ostream& operator<<(std::ostream& O, const ray<U>&r)
	{	{
	return O << r.pos.x << " " << r.pos.y << " " << r.pos.z << " " << r.dir .x << " " << r.dir.y << " " << r.dir.z << " ";	return O << r.pos.x << " " << r.pos.y << " " << r.pos.z << " " << r.dir .x << " " << r.dir.y << " " << r.dir.z << " ";
	}	}

	template <typename U>	template <typename U>

	std::istream& operator>>(std::istream& I, ray<U>& r)	std::istream& operator>>(std::istream& I, ray<U>&r)
	{	{
	return I >> r.pos.x >> r.pos.y >> r.pos.z >> r.dir.x >> r.dir.y >> r.di r.z;	return I >> r.pos.x >> r.pos.y >> r.pos.z >> r.dir.x >> r.dir.y >> r.di r.z;
	}	}

	/// \brief serialize in column order! This is the format transformations ar e passed across the network	/// \brief serialize in column order! This is the format transformations ar e passed across the network
	template <typename U>	template <typename U>

	std::ostream& operator<<(std::ostream& O, const RaveTransformMatrix<U>& v)	std::ostream& operator<<(std::ostream& O, const RaveTransformMatrix<U>&v)
	{	{
	return O << v.m[0] << " " << v.m[4] << " " << v.m[8] << " " << v.m[1] < < " " << v.m[5] << " " << v.m[9] << " " << v.m[2] << " " << v.m[6] << " " < < v.m[10] << " " << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";	return O << v.m[0] << " " << v.m[4] << " " << v.m[8] << " " << v.m[1] < < " " << v.m[5] << " " << v.m[9] << " " << v.m[2] << " " << v.m[6] << " " < < v.m[10] << " " << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
	}	}

	/// \brief de-serialize in column order! This is the format transformations are passed across the network	/// \brief de-serialize in column order! This is the format transformations are passed across the network
	template <typename U>	template <typename U>

	std::istream& operator>>(std::istream& I, RaveTransformMatrix<U>& v)	std::istream& operator>>(std::istream& I, RaveTransformMatrix<U>&v)
	{	{
	return I >> v.m[0] >> v.m[4] >> v.m[8] >> v.m[1] >> v.m[5] >> v.m[9] >> v.m[2] >> v.m[6] >> v.m[10] >> v.trans.x >> v.trans.y >> v.trans.z;	return I >> v.m[0] >> v.m[4] >> v.m[8] >> v.m[1] >> v.m[5] >> v.m[9] >> v.m[2] >> v.m[6] >> v.m[10] >> v.trans.x >> v.trans.y >> v.trans.z;
	}	}

	//@}	//@}

	/// \brief Converts an axis-angle rotation into a quaternion.	/// \brief Converts an axis-angle rotation into a quaternion.
	///	///
	/// \ingroup affine_math	/// \ingroup affine_math
	/// \param axis unit axis, 3 values	/// \param axis unit axis, 3 values

	skipping to change at line 805	skipping to change at line 906
	RaveVector<T> qb, qm;	RaveVector<T> qb, qm;
	if( quat0.dot(quat1) < 0 ) {	if( quat0.dot(quat1) < 0 ) {
	qb = -quat1;	qb = -quat1;
	}	}
	else {	else {
	qb = quat1;	qb = quat1;
	}	}
	// Calculate angle between them.	// Calculate angle between them.
	T cosHalfTheta = quat0.w * qb.w + quat0.x * qb.x + quat0.y * qb.y + qua t0.z * qb.z;	T cosHalfTheta = quat0.w * qb.w + quat0.x * qb.x + quat0.y * qb.y + qua t0.z * qb.z;
	// if quat0=qb or quat0=-qb then theta = 0 and we can return quat0	// if quat0=qb or quat0=-qb then theta = 0 and we can return quat0

	if (MATH_FABS(cosHalfTheta) >= 1.0){	if (MATH_FABS(cosHalfTheta) >= 1.0) {
	qm.w = quat0.w;qm.x = quat0.x;qm.y = quat0.y;qm.z = quat0.z;	qm.w = quat0.w; qm.x = quat0.x; qm.y = quat0.y; qm.z = quat0.z;
	return qm;	return qm;
	}	}
	// Calculate temporary values.	// Calculate temporary values.
	T halfTheta = MATH_ACOS(cosHalfTheta);	T halfTheta = MATH_ACOS(cosHalfTheta);
	T sinHalfTheta = MATH_SQRT(1 - cosHalfTheta*cosHalfTheta);	T sinHalfTheta = MATH_SQRT(1 - cosHalfTheta*cosHalfTheta);
	// if theta = 180 degrees then result is not fully defined	// if theta = 180 degrees then result is not fully defined
	// we could rotate around any axis normal to quat0 or qb	// we could rotate around any axis normal to quat0 or qb

	if (MATH_FABS(sinHalfTheta) < 1e-7f){ // fabs is floating point absolut e	if (MATH_FABS(sinHalfTheta) < 1e-7f) { // fabs is floating point absolu te
	qm.w = (quat0.w * 0.5f + qb.w * 0.5f);	qm.w = (quat0.w * 0.5f + qb.w * 0.5f);
	qm.x = (quat0.x * 0.5f + qb.x * 0.5f);	qm.x = (quat0.x * 0.5f + qb.x * 0.5f);
	qm.y = (quat0.y * 0.5f + qb.y * 0.5f);	qm.y = (quat0.y * 0.5f + qb.y * 0.5f);
	qm.z = (quat0.z * 0.5f + qb.z * 0.5f);	qm.z = (quat0.z * 0.5f + qb.z * 0.5f);
	return qm;	return qm;
	}	}
	T ratioA = MATH_SIN((1 - t) * halfTheta) / sinHalfTheta;	T ratioA = MATH_SIN((1 - t) * halfTheta) / sinHalfTheta;
	T ratioB = MATH_SIN(t * halfTheta) / sinHalfTheta;	T ratioB = MATH_SIN(t * halfTheta) / sinHalfTheta;
	//calculate Quaternion.	//calculate Quaternion.
	qm.w = (quat0.w * ratioA + qb.w * ratioB);	qm.w = (quat0.w * ratioA + qb.w * ratioB);

	skipping to change at line 1000	skipping to change at line 1101
	}	}

	/// \brief Tests a point inside a 3D quadrilateral.	/// \brief Tests a point inside a 3D quadrilateral.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	inline int insideQuadrilateral(const RaveVector<T>& v, const RaveVector<T>& verts)	inline int insideQuadrilateral(const RaveVector<T>& v, const RaveVector<T>& verts)
	{	{
	RaveVector<T> v4,v5;	RaveVector<T> v4,v5;
	T m1,m2;	T m1,m2;
	T anglesum=0,costheta;	T anglesum=0,costheta;

	for (int i=0;i<4;i++) {	for (int i=0; i<4; i++) {
	v4.x = verts[i].x - v->x;	v4.x = verts[i].x - v->x;
	v4.y = verts[i].y - v->y;	v4.y = verts[i].y - v->y;
	v4.z = verts[i].z - v->z;	v4.z = verts[i].z - v->z;
	v5.x = verts[(i+1)%4].x - v->x;	v5.x = verts[(i+1)%4].x - v->x;
	v5.y = verts[(i+1)%4].y - v->y;	v5.y = verts[(i+1)%4].y - v->y;
	v5.z = verts[(i+1)%4].z - v->z;	v5.z = verts[(i+1)%4].z - v->z;
	m1 = v4.lengthsqr3();	m1 = v4.lengthsqr3();
	m2 = v5.lengthsqr3();	m2 = v5.lengthsqr3();
	if (m1m2 <= std::numeric_limits<T>::epsilon()std::numeric_limits< T>::epsilon()) {	if (m1m2 <= std::numeric_limits<T>::epsilon()std::numeric_limits< T>::epsilon()) {
	return(1); // on a vertex, consider this inside	return(1); // on a vertex, consider this inside

	skipping to change at line 1030	skipping to change at line 1131

	/// \brief Tests a point insdie a 3D triangle.	/// \brief Tests a point insdie a 3D triangle.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	inline int insideTriangle(const RaveVector<T> v, const triangle<T>& tri)	inline int insideTriangle(const RaveVector<T> v, const triangle<T>& tri)
	{	{
	RaveVector<T> v4,v5;	RaveVector<T> v4,v5;
	T m1,m2;	T m1,m2;
	T anglesum=0.0;	T anglesum=0.0;
	T costheta;	T costheta;

	for (int i=0;i<3;i++) {	for (int i=0; i<3; i++) {
	v4.x = tri[i].x - v->x;	v4.x = tri[i].x - v->x;
	v4.y = tri[i].y - v->y;	v4.y = tri[i].y - v->y;
	v4.z = tri[i].z - v->z;	v4.z = tri[i].z - v->z;
	v5.x = tri[(i+1)%3].x - v->x;	v5.x = tri[(i+1)%3].x - v->x;
	v5.y = tri[(i+1)%3].y - v->y;	v5.y = tri[(i+1)%3].y - v->y;
	v5.z = tri[(i+1)%3].z - v->z;	v5.z = tri[(i+1)%3].z - v->z;
	m1 = v4.lengthsqr3();	m1 = v4.lengthsqr3();
	m2 = v5.lengthsqr3();	m2 = v5.lengthsqr3();
	if (m1m2 <= std::numeric_limits<T>::epsilon()std::numeric_limits< T>::epsilon()) {	if (m1m2 <= std::numeric_limits<T>::epsilon()std::numeric_limits< T>::epsilon()) {

	return(1); // on a vertex, consider this inside	return(1); // on a vertex, consider this inside
	}	}
	else {	else {

	costheta = v4.dot3(v5)/MATH_SQRT(m1*m2);	costheta = v4.dot3(v5)/MATH_SQRT(m1*m2);
	}	}
	anglesum += acos(costheta);	anglesum += acos(costheta);
	}	}
	T diff = anglesum - (T)2.0 * M_PI;	T diff = anglesum - (T)2.0 * M_PI;
	return RaveFabs(diff) <= std::numeric_limits<T>::epsilon();	return RaveFabs(diff) <= std::numeric_limits<T>::epsilon();
	}	}

	/// \brief Test collision of a ray with an axis aligned bounding box.	/// \brief Test collision of a ray with an axis aligned bounding box.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	inline bool RayAABBTest(const ray<T>& r, const aabb<T>& ab)	inline bool RayAABBTest(const ray<T>& r, const aabb<T>& ab)
	{	{
	RaveVector<T> vd, vpos = r.pos - ab.pos;	RaveVector<T> vd, vpos = r.pos - ab.pos;

	if( MATH_FABS(vpos.x) > ab.extents.x && r.dir.x* vpos.x > 0.0f)	if((MATH_FABS(vpos.x) > ab.extents.x)&&(r.dir.x* vpos.x > 0.0f))
	return false;	return false;

	if( MATH_FABS(vpos.y) > ab.extents.y && r.dir.y * vpos.y > 0.0f)	if((MATH_FABS(vpos.y) > ab.extents.y)&&(r.dir.y * vpos.y > 0.0f))
	return false;	return false;

	if( MATH_FABS(vpos.z) > ab.extents.z && r.dir.z * vpos.z > 0.0f)	if((MATH_FABS(vpos.z) > ab.extents.z)&&(r.dir.z * vpos.z > 0.0f))
	return false;	return false;
	vd = r.dir.cross(vpos);	vd = r.dir.cross(vpos);
	if( MATH_FABS(vd.x) > ab.extents.y * MATH_FABS(r.dir.z) + ab.extents.z * MATH_FABS(r.dir.y) )	if( MATH_FABS(vd.x) > ab.extents.y * MATH_FABS(r.dir.z) + ab.extents.z * MATH_FABS(r.dir.y) )
	return false;	return false;
	if( MATH_FABS(vd.y) > ab.extents.x * MATH_FABS(r.dir.z) + ab.extents.z * MATH_FABS(r.dir.x) )	if( MATH_FABS(vd.y) > ab.extents.x * MATH_FABS(r.dir.z) + ab.extents.z * MATH_FABS(r.dir.x) )
	return false;	return false;
	if( MATH_FABS(vd.z) > ab.extents.x * MATH_FABS(r.dir.y) + ab.extents.y * MATH_FABS(r.dir.x) )	if( MATH_FABS(vd.z) > ab.extents.x * MATH_FABS(r.dir.y) + ab.extents.y * MATH_FABS(r.dir.x) )
	return false;	return false;
	return true;	return true;
	}	}

	/// \brief Test collision of a ray and an oriented bounding box.	/// \brief Test collision of a ray and an oriented bounding box.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	inline bool RayOBBTest(const ray<T>& r, const obb<T>& o)	inline bool RayOBBTest(const ray<T>& r, const obb<T>& o)
	{	{
	RaveVector<T> vpos, vdir, vd;	RaveVector<T> vpos, vdir, vd;
	vd = r.pos - o.pos;	vd = r.pos - o.pos;
	vpos.x = vd.dot3(o.right);	vpos.x = vd.dot3(o.right);
	vdir.x = r.dir.dot3(o.right);	vdir.x = r.dir.dot3(o.right);

	if( MATH_FABS(vpos.x) > o.extents.x && vdir.x* vpos.x > 0.0f) {	if((MATH_FABS(vpos.x) > o.extents.x)&&(vdir.x* vpos.x > 0.0f)) {
	return false;	return false;
	}	}
	vpos.y = vd.dot3(o.up);	vpos.y = vd.dot3(o.up);
	vdir.y = r.dir.dot3(o.up);	vdir.y = r.dir.dot3(o.up);

	if( MATH_FABS(vpos.y) > o.extents.y && vdir.y * vpos.y > 0.0f) {	if((MATH_FABS(vpos.y) > o.extents.y)&&(vdir.y * vpos.y > 0.0f)) {
	return false;	return false;
	}	}
	vpos.z = vd.dot3(o.dir);	vpos.z = vd.dot3(o.dir);
	vdir.z = r.dir.dot3(o.dir);	vdir.z = r.dir.dot3(o.dir);

	if( MATH_FABS(vpos.z) > o.extents.z && vdir.z * vpos.z > 0.0f) {	if((MATH_FABS(vpos.z) > o.extents.z)&&(vdir.z * vpos.z > 0.0f)) {
	return false;	return false;
	}	}
	cross3(vd, vdir, vpos);	cross3(vd, vdir, vpos);

	if( MATH_FABS(vd.x) > o.extents.y * MATH_FABS(vdir.z) + o.extents.z * M	if((MATH_FABS(vd.x) > o.extents.y * MATH_FABS(vdir.z) + o.extents.z * M
	ATH_FABS(vdir.y) \|\|	ATH_FABS(vdir.y))\|\|
	MATH_FABS(vd.y) > o.extents.x * MATH_FABS(vdir.z) + o.extents.z * M	( MATH_FABS(vd.y) > o.extents.x * MATH_FABS(vdir.z) + o.extents.z *
	ATH_FABS(vdir.x) \|\|	MATH_FABS(vdir.x)) \|\|
	MATH_FABS(vd.z) > o.extents.x * MATH_FABS(vdir.y) + o.extents.y * M	( MATH_FABS(vd.z) > o.extents.x * MATH_FABS(vdir.y) + o.extents.y *
	ATH_FABS(vdir.x) ) {	MATH_FABS(vdir.x)) ) {
	return false;	return false;
	}	}
	return true;	return true;
	}	}

	//bool RayFaceTest(const RAY& r, const FACE& f)	//bool RayFaceTest(const RAY& r, const FACE& f)
	//{	//{
	// float t = D3DXVec3Dot(&f.plane.vNorm, &(f.v1 - r.vPos) ) / D3DXVec3D ot(&f.plane.vNorm, &r.vDir);	// float t = D3DXVec3Dot(&f.plane.vNorm, &(f.v1 - r.vPos) ) / D3DXVec3D ot(&f.plane.vNorm, &r.vDir);
	//	//
	// if(t <= 0.0f) return false;	// if(t <= 0.0f) return false;

	skipping to change at line 1430	skipping to change at line 1531
	inline bool TriTriCollision(const RaveVector<T>& u1, const RaveVector<T>& u 2, const RaveVector<T>& u3, const RaveVector<T>& v1, const RaveVector<T>& v 2, const RaveVector<T>& v3, RaveVector<T>& contactpos, RaveVector<T>& conta ctnorm)	inline bool TriTriCollision(const RaveVector<T>& u1, const RaveVector<T>& u 2, const RaveVector<T>& u3, const RaveVector<T>& v1, const RaveVector<T>& v 2, const RaveVector<T>& v3, RaveVector<T>& contactpos, RaveVector<T>& conta ctnorm)
	{	{
	// triangle triangle collision test - by Rosen Diankov	// triangle triangle collision test - by Rosen Diankov

	// first see if the faces intersect the planes	// first see if the faces intersect the planes
	// for the face to be intersecting the plane, one of its	// for the face to be intersecting the plane, one of its
	// vertices must be on the opposite side of the plane	// vertices must be on the opposite side of the plane
	char b = 0;	char b = 0;
	RaveVector<T> u12 = u2 - u1, u23 = u3 - u2, u31 = u1 - u3;	RaveVector<T> u12 = u2 - u1, u23 = u3 - u2, u31 = u1 - u3;
	RaveVector<T> v12 = v2 - v1, v23 = v3 - v2, v31 = v1 - v3;	RaveVector<T> v12 = v2 - v1, v23 = v3 - v2, v31 = v1 - v3;

	RaveVector<T> vedges[3] = {v12, v23, v31};	RaveVector<T> vedges[3] = { v12, v23, v31};
	RaveVector<T> unorm, vnorm;	RaveVector<T> unorm, vnorm;
	unorm = u31.cross(u12);	unorm = u31.cross(u12);
	unorm.w = -unorm.dot3(u1);	unorm.w = -unorm.dot3(u1);
	vnorm = v31.cross(v12);	vnorm = v31.cross(v12);
	vnorm.w = -vnorm.dot3(v1);	vnorm.w = -vnorm.dot3(v1);
	if( vnorm.dot3(u1) + vnorm.w > 0 ) {	if( vnorm.dot3(u1) + vnorm.w > 0 ) {
	b \|= 1;	b \|= 1;
	}	}
	if( vnorm.dot3(u2) + vnorm.w > 0 ) {	if( vnorm.dot3(u2) + vnorm.w > 0 ) {
	b \|= 2;	b \|= 2;
	}	}
	if( vnorm.dot3(u3) + vnorm.w > 0 ) {	if( vnorm.dot3(u3) + vnorm.w > 0 ) {
	b \|= 4;	b \|= 4;
	}	}

	if(b == 7 \|\| b == 0) {	if((b == 7)\|\|(b == 0)) {
	return false;	return false;
	}	}
	// now get segment from f1 when it crosses f2's plane	// now get segment from f1 when it crosses f2's plane
	// note that b gives us information on which edges actually intersected	// note that b gives us information on which edges actually intersected
	// so figure out the point that is alone on one side of the plane	// so figure out the point that is alone on one side of the plane
	// then get the segment	// then get the segment
	RaveVector<T> p1, p2;	RaveVector<T> p1, p2;
	const RaveVector<T>* pu=NULL;	const RaveVector<T>* pu=NULL;
	switch(b) {	switch(b) {
	case 1:	case 1:

	skipping to change at line 1481	skipping to change at line 1582
	p2 = u2 - u3;	p2 = u2 - u3;
	break;	break;
	}	}

	T t = vnorm.dot3(*pu)+vnorm.w;	T t = vnorm.dot3(*pu)+vnorm.w;
	p1 = pu - p1 (t / vnorm.dot3(p1));	p1 = pu - p1 (t / vnorm.dot3(p1));
	p2 = pu - p2 (t / vnorm.dot3(p2));	p2 = pu - p2 (t / vnorm.dot3(p2));

	// go through each of the segments in v2 and clip	// go through each of the segments in v2 and clip
	RaveVector<T> vcross;	RaveVector<T> vcross;

	const RaveVector<T>* pv[] = {&v1, &v2, &v3, &v1};	const RaveVector<T>* pv[] = { &v1, &v2, &v3, &v1};

	for(int i = 0; i < 3; ++i) {	for(int i = 0; i < 3; ++i) {
	const RaveVector<T>* pprev = pv[i];	const RaveVector<T>* pprev = pv[i];
	RaveVector<T> q1 = p1 - *pprev;	RaveVector<T> q1 = p1 - *pprev;
	RaveVector<T> q2 = p2 - *pprev;	RaveVector<T> q2 = p2 - *pprev;
	vcross = vedges[i].cross(vnorm);	vcross = vedges[i].cross(vnorm);
	T t1 = q1.dot3(vcross);	T t1 = q1.dot3(vcross);
	T t2 = q2.dot3(vcross);	T t2 = q2.dot3(vcross);

	// line segment is out of face	// line segment is out of face

	if( t1 >= 0 && t2 >= 0 ) {	if((t1 >= 0)&&(t2 >= 0)) {
	return false;	return false;
	}	}

	if( t1 > 0 && t2 < 0 ) {	if((t1 > 0)&&(t2 < 0)) {
	// keep second point, clip first	// keep second point, clip first
	RaveVector<T> dq = q2-q1;	RaveVector<T> dq = q2-q1;
	p1 -= dq*(t1/dq.dot3(vcross));	p1 -= dq*(t1/dq.dot3(vcross));
	}	}

	else if( t1 < 0 && t2 > 0 ) {	else if((t1 < 0)&&(t2 > 0)) {
	// keep first point, clip second	// keep first point, clip second
	RaveVector<T> dq = q1-q2;	RaveVector<T> dq = q1-q2;
	p2 -= dq*(t2/dq.dot3(vcross));	p2 -= dq*(t2/dq.dot3(vcross));
	}	}
	}	}

	contactpos = 0.5f * (p1 + p2);	contactpos = 0.5f * (p1 + p2);
	contactnorm = vnorm.normalize3();	contactnorm = vnorm.normalize3();
	return true;	return true;
	}	}

End of changes. 82 change blocks.
	166 lines changed or deleted	256 lines changed or added

	mathextra.h	mathextra.h

	// Copyright (C) 2006-2010 Rosen Diankov (rosen.diankov@gmail.com)	// -- coding: utf-8 --
		// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 41	skipping to change at line 42
	#endif	#endif

	#include <cmath>	#include <cmath>
	#include <climits>	#include <climits>

	namespace OpenRAVE {	namespace OpenRAVE {

	/// Extra math routines that are useful to have but don't really belong any where.	/// Extra math routines that are useful to have but don't really belong any where.
	namespace mathextra {	namespace mathextra {


	#define distinctRoots 0 // roots r0 < r1 < r2	#define distinctRoots 0 // roots r0 < r1 < r2
	#define singleRoot 1 // root r0	#define singleRoot 1 // root r0
	#define floatRoot01 2 // roots r0 = r1 < r	#define floatRoot01 2 // roots r0 = r1 < r2
	2	#define floatRoot12 4 // roots r0 < r1 = r2
	#define floatRoot12 4 // roots r0 < r1 = r	#define tripleRoot 6 // roots r0 = r1 = r2
	2
	#define tripleRoot 6 // roots r0 = r1 = r
	2

	// multiplies 4x4 matrices	// multiplies 4x4 matrices
	inline float* mult4(float* pfres, const float* pf1, const float* pf2);	inline float* mult4(float* pfres, const float* pf1, const float* pf2);

	// pf1^T * pf2	// pf1^T * pf2
	inline float* multtrans3(float* pfres, const float* pf1, const float* pf2);	inline float* multtrans3(float* pfres, const float* pf1, const float* pf2);
	inline float* multtrans4(float* pfres, const float* pf1, const float* pf2);	inline float* multtrans4(float* pfres, const float* pf1, const float* pf2);
	inline float* transnorm3(float* pfout, const float* pfmat, const float* pf) ;	inline float* transnorm3(float* pfout, const float* pfmat, const float* pf) ;

	inline float* transpose3(const float* pf, float* pfres);	inline float* transpose3(const float* pf, float* pfres);

	skipping to change at line 153	skipping to change at line 154
	/// SVD of a 3x3 matrix A such that A = Udiag(D)V'	/// SVD of a 3x3 matrix A such that A = Udiag(D)V'
	/// The row stride for all matrices is 3*sizeof(T) bytes	/// The row stride for all matrices is 3*sizeof(T) bytes
	/// \param[in] A 3x3 matrix	/// \param[in] A 3x3 matrix
	/// \param[out] U 3x3 matrix	/// \param[out] U 3x3 matrix
	/// \param[out] D 3x1 matrix	/// \param[out] D 3x1 matrix
	/// \param[out] V 3x3 matrix	/// \param[out] V 3x3 matrix
	template <typename T> inline void svd3(const T* A, T* U, T* D, T* V);	template <typename T> inline void svd3(const T* A, T* U, T* D, T* V);

	template <typename T> inline void mult(T* pf, T fa, int r)	template <typename T> inline void mult(T* pf, T fa, int r)
	{	{

	MATH_ASSERT( pf != NULL );	MATH_ASSERT( pf != NULL );
	while(r > 0) {	while(r > 0) {
	--r;	--r;
	pf[r] *= fa;	pf[r] *= fa;
	}	}
	}	}

	template <typename T> int Min(T* pts, int stride, int numPts); // returns t he index, stride in units of T	template <typename T> int Min(T* pts, int stride, int numPts); // returns t he index, stride in units of T
	template <typename T> int Max(T* pts, int stride, int numPts); // returns t he index	template <typename T> int Max(T* pts, int stride, int numPts); // returns t he index

	// multiplies a matrix by a scalar	// multiplies a matrix by a scalar
	template <typename T> inline void mult(T* pf, T fa, int r);	template <typename T> inline void mult(T* pf, T fa, int r);

	// multiplies a r1xc1 by c1xc2 matrix into pfres, if badd is true adds the result to pfres	// multiplies a r1xc1 by c1xc2 matrix into pfres, if badd is true adds the result to pfres
	// does not handle cases where pfres is equal to pf1 or pf2, use multtox fo r those cases	// does not handle cases where pfres is equal to pf1 or pf2, use multtox fo r those cases

	skipping to change at line 271	skipping to change at line 272
	return 1;	return 1;
	}	}
	// two roots	// two roots
	d = sqrt(d);	d = sqrt(d);
	r1 = (-b+d)/(2*a);	r1 = (-b+d)/(2*a);
	r2 = c/(a*r1);	r2 = c/(a*r1);
	return 2;	return 2;
	}	}

	#define MULT3(stride) { \	#define MULT3(stride) { \

	pfres2[0stride+0] = pf1[0stride+0]pf2[0stride+0]+pf1[0*stride+1]	pfres2[0stride+0] = pf1[0stride+0]pf2[0stride+0]+pf1[0*stride+1
	pf2[1stride+0]+pf1[0stride+2]pf2[2*stride+0]; \	]pf2[1stride+0]+pf1[0stride+2]pf2[2*stride+0]; \
	pfres2[0stride+1] = pf1[0stride+0]pf2[0stride+1]+pf1[0*stride+1]	pfres2[0stride+1] = pf1[0stride+0]pf2[0stride+1]+pf1[0*stride+1
	pf2[1stride+1]+pf1[0stride+2]pf2[2*stride+1]; \	]pf2[1stride+1]+pf1[0stride+2]pf2[2*stride+1]; \
	pfres2[0stride+2] = pf1[0stride+0]pf2[0stride+2]+pf1[0*stride+1]	pfres2[0stride+2] = pf1[0stride+0]pf2[0stride+2]+pf1[0*stride+1
	pf2[1stride+2]+pf1[0stride+2]pf2[2*stride+2]; \	]pf2[1stride+2]+pf1[0stride+2]pf2[2*stride+2]; \
	pfres2[1stride+0] = pf1[1stride+0]pf2[0stride+0]+pf1[1*stride+1]	pfres2[1stride+0] = pf1[1stride+0]pf2[0stride+0]+pf1[1*stride+1
	pf2[1stride+0]+pf1[1stride+2]pf2[2*stride+0]; \	]pf2[1stride+0]+pf1[1stride+2]pf2[2*stride+0]; \
	pfres2[1stride+1] = pf1[1stride+0]pf2[0stride+1]+pf1[1*stride+1]	pfres2[1stride+1] = pf1[1stride+0]pf2[0stride+1]+pf1[1*stride+1
	pf2[1stride+1]+pf1[1stride+2]pf2[2*stride+1]; \	]pf2[1stride+1]+pf1[1stride+2]pf2[2*stride+1]; \
	pfres2[1stride+2] = pf1[1stride+0]pf2[0stride+2]+pf1[1*stride+1]	pfres2[1stride+2] = pf1[1stride+0]pf2[0stride+2]+pf1[1*stride+1
	pf2[1stride+2]+pf1[1stride+2]pf2[2*stride+2]; \	]pf2[1stride+2]+pf1[1stride+2]pf2[2*stride+2]; \
	pfres2[2stride+0] = pf1[2stride+0]pf2[0stride+0]+pf1[2*stride+1]	pfres2[2stride+0] = pf1[2stride+0]pf2[0stride+0]+pf1[2*stride+1
	pf2[1stride+0]+pf1[2stride+2]pf2[2*stride+0]; \	]pf2[1stride+0]+pf1[2stride+2]pf2[2*stride+0]; \
	pfres2[2stride+1] = pf1[2stride+0]pf2[0stride+1]+pf1[2*stride+1]	pfres2[2stride+1] = pf1[2stride+0]pf2[0stride+1]+pf1[2*stride+1
	pf2[1stride+1]+pf1[2stride+2]pf2[2*stride+1]; \	]pf2[1stride+1]+pf1[2stride+2]pf2[2*stride+1]; \
	pfres2[2stride+2] = pf1[2stride+0]pf2[0stride+2]+pf1[2*stride+1]	pfres2[2stride+2] = pf1[2stride+0]pf2[0stride+2]+pf1[2*stride+1
	pf2[1stride+2]+pf1[2stride+2]pf2[2*stride+2]; \	]pf2[1stride+2]+pf1[2stride+2]pf2[2*stride+2]; \
	}	}

	/// mult3 with a 3x3 matrix whose row stride is 16 bytes	/// mult3 with a 3x3 matrix whose row stride is 16 bytes
	template <typename T>	template <typename T>
	inline T* _mult3_s4(T* pfres, const T* pf1, const T* pf2)	inline T* _mult3_s4(T* pfres, const T* pf1, const T* pf2)
	{	{
	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL );

	T* pfres2;	T* pfres2;

	if( pfres == pf1 \|\| pfres == pf2 ) pfres2 = (T)alloca(12 sizeof(T));	if((pfres == pf1)\|\|(pfres == pf2)) pfres2 = (T)alloca(12 sizeof(T));
	else pfres2 = pfres;	else pfres2 = pfres;

	MULT3(4);	MULT3(4);
	if( pfres2 != pfres ) memcpy(pfres, pfres2, 12*sizeof(T));	if( pfres2 != pfres ) memcpy(pfres, pfres2, 12*sizeof(T));
	return pfres;	return pfres;
	}	}

	/// mult3 with a 3x3 matrix whose row stride is 12 bytes	/// mult3 with a 3x3 matrix whose row stride is 12 bytes
	template <typename T>	template <typename T>
	inline T* _mult3_s3(T* pfres, const T* pf1, const T* pf2)	inline T* _mult3_s3(T* pfres, const T* pf1, const T* pf2)
	{	{
	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL );

	T* pfres2;	T* pfres2;

	if( pfres == pf1 \|\| pfres == pf2 ) pfres2 = (T)alloca(9 sizeof(T));	if((pfres == pf1)\|\|(pfres == pf2)) pfres2 = (T)alloca(9 sizeof(T));
	else pfres2 = pfres;	else pfres2 = pfres;

	MULT3(3);	MULT3(3);

	if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*sizeof(T));	if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*sizeof(T));

	return pfres;	return pfres;
	}	}

	// mult4	// mult4
	template <typename T>	template <typename T>
	inline T* _mult4(T* pfres, const T* p1, const T* p2)	inline T* _mult4(T* pfres, const T* p1, const T* p2)
	{	{
	MATH_ASSERT( pfres != NULL && p1 != NULL && p2 != NULL );	MATH_ASSERT( pfres != NULL && p1 != NULL && p2 != NULL );

	T* pfres2;	T* pfres2;

	if( pfres == p1 \|\| pfres == p2 ) pfres2 = (T)alloca(16 sizeof(T));	if((pfres == p1)\|\|(pfres == p2)) pfres2 = (T)alloca(16 sizeof(T));
	else pfres2 = pfres;	else pfres2 = pfres;

	pfres2[04+0] = p1[04+0]p2[04+0] + p1[04+1]p2[14+0] + p1[04+2]p 2[24+0] + p1[04+3]p2[3*4+0];	pfres2[04+0] = p1[04+0]p2[04+0] + p1[04+1]p2[14+0] + p1[04+2]p 2[24+0] + p1[04+3]p2[3*4+0];
	pfres2[04+1] = p1[04+0]p2[04+1] + p1[04+1]p2[14+1] + p1[04+2]p 2[24+1] + p1[04+3]p2[3*4+1];	pfres2[04+1] = p1[04+0]p2[04+1] + p1[04+1]p2[14+1] + p1[04+2]p 2[24+1] + p1[04+3]p2[3*4+1];
	pfres2[04+2] = p1[04+0]p2[04+2] + p1[04+1]p2[14+2] + p1[04+2]p 2[24+2] + p1[04+3]p2[3*4+2];	pfres2[04+2] = p1[04+0]p2[04+2] + p1[04+1]p2[14+2] + p1[04+2]p 2[24+2] + p1[04+3]p2[3*4+2];
	pfres2[04+3] = p1[04+0]p2[04+3] + p1[04+1]p2[14+3] + p1[04+2]p 2[24+3] + p1[04+3]p2[3*4+3];	pfres2[04+3] = p1[04+0]p2[04+3] + p1[04+1]p2[14+3] + p1[04+2]p 2[24+3] + p1[04+3]p2[3*4+3];

	pfres2[14+0] = p1[14+0]p2[04+0] + p1[14+1]p2[14+0] + p1[14+2]p 2[24+0] + p1[14+3]p2[3*4+0];	pfres2[14+0] = p1[14+0]p2[04+0] + p1[14+1]p2[14+0] + p1[14+2]p 2[24+0] + p1[14+3]p2[3*4+0];
	pfres2[14+1] = p1[14+0]p2[04+1] + p1[14+1]p2[14+1] + p1[14+2]p 2[24+1] + p1[14+3]p2[3*4+1];	pfres2[14+1] = p1[14+0]p2[04+1] + p1[14+1]p2[14+1] + p1[14+2]p 2[24+1] + p1[14+3]p2[3*4+1];
	pfres2[14+2] = p1[14+0]p2[04+2] + p1[14+1]p2[14+2] + p1[14+2]p 2[24+2] + p1[14+3]p2[3*4+2];	pfres2[14+2] = p1[14+0]p2[04+2] + p1[14+1]p2[14+2] + p1[14+2]p 2[24+2] + p1[14+3]p2[3*4+2];

	skipping to change at line 392	skipping to change at line 393
	}	}
	}	}

	return pfres;	return pfres;
	}	}

	/// \brief Compute the determinant of a 3x3 matrix whose row stride stride elements.	/// \brief Compute the determinant of a 3x3 matrix whose row stride stride elements.
	template <typename T> inline T matrixdet3(const T* pf, int stride)	template <typename T> inline T matrixdet3(const T* pf, int stride)
	{	{
	return pf[0stride+2] (pf[1stride + 0] pf[2stride + 1] - pf[1str ide + 1] * pf[2*stride + 0]) +	return pf[0stride+2] (pf[1stride + 0] pf[2stride + 1] - pf[1str ide + 1] * pf[2*stride + 0]) +

	pf[1stride+2] (pf[0stride + 1] pf[2stride + 0] - pf[0stride	pf[1stride+2] (pf[0stride + 1] pf[2stride + 0] - pf[0str
	+ 0] * pf[2*stride + 1]) +	ide + 0] * pf[2*stride + 1]) +
	pf[2stride+2] (pf[0stride + 0] pf[1stride + 1] - pf[0stride	pf[2stride+2] (pf[0stride + 0] pf[1stride + 1] - pf[0str
	+ 1] * pf[1*stride + 0]);	ide + 1] * pf[1*stride + 0]);
	}	}

	/** \brief 3x3 matrix inverse.	/** \brief 3x3 matrix inverse.

	\param[in] pf the input 3x3 matrix	\param[in] pf the input 3x3 matrix
	\param[out] pf the result of the operation, can be the same matrix as p f	\param[out] pf the result of the operation, can be the same matrix as p f
	\param[out] pfdet if not NULL, fills it with the determinant of the sou rce matrix	\param[out] pfdet if not NULL, fills it with the determinant of the sou rce matrix
	\param[in] stride the stride in elements between elements.	\param[in] stride the stride in elements between elements.

	*/	*/
	template <typename T>	template <typename T>
	inline T* _inv3(const T* pf, T* pfres, T* pfdet, int stride)	inline T* _inv3(const T* pf, T* pfres, T* pfdet, int stride)
	{	{

	T* pfres2;	T* pfres2;
	if( pfres == pf ) pfres2 = (T)alloca(3 stride * sizeof(T));	if( pfres == pf ) pfres2 = (T)alloca(3 stride * sizeof(T));
	else pfres2 = pfres;	else pfres2 = pfres;


	// inverse = C^t / det(pf) where C is the matrix of coefficients	// inverse = C^t / det(pf) where C is the matrix of coefficients


	// calc C^t	// calc C^t
	pfres2[0stride + 0] = pf[1stride + 1] * pf[2stride + 2] - pf[1st	pfres2[0stride + 0] = pf[1stride + 1] * pf[2stride + 2] - pf[1strid
	ride + 2] * pf[2*stride + 1];	e + 2] * pf[2*stride + 1];
	pfres2[0stride + 1] = pf[0stride + 2] * pf[2stride + 1] - pf[0st	pfres2[0stride + 1] = pf[0stride + 2] * pf[2stride + 1] - pf[0strid
	ride + 1] * pf[2*stride + 2];	e + 1] * pf[2*stride + 2];
	pfres2[0stride + 2] = pf[0stride + 1] * pf[1stride + 2] - pf[0st	pfres2[0stride + 2] = pf[0stride + 1] * pf[1stride + 2] - pf[0strid
	ride + 2] * pf[1*stride + 1];	e + 2] * pf[1*stride + 1];
	pfres2[1stride + 0] = pf[1stride + 2] * pf[2stride + 0] - pf[1st	pfres2[1stride + 0] = pf[1stride + 2] * pf[2stride + 0] - pf[1strid
	ride + 0] * pf[2*stride + 2];	e + 0] * pf[2*stride + 2];
	pfres2[1stride + 1] = pf[0stride + 0] * pf[2stride + 2] - pf[0st	pfres2[1stride + 1] = pf[0stride + 0] * pf[2stride + 2] - pf[0strid
	ride + 2] * pf[2*stride + 0];	e + 2] * pf[2*stride + 0];
	pfres2[1stride + 2] = pf[0stride + 2] * pf[1stride + 0] - pf[0st	pfres2[1stride + 2] = pf[0stride + 2] * pf[1stride + 0] - pf[0strid
	ride + 0] * pf[1*stride + 2];	e + 0] * pf[1*stride + 2];
	pfres2[2stride + 0] = pf[1stride + 0] * pf[2stride + 1] - pf[1st	pfres2[2stride + 0] = pf[1stride + 0] * pf[2stride + 1] - pf[1strid
	ride + 1] * pf[2*stride + 0];	e + 1] * pf[2*stride + 0];
	pfres2[2stride + 1] = pf[0stride + 1] * pf[2stride + 0] - pf[0st	pfres2[2stride + 1] = pf[0stride + 1] * pf[2stride + 0] - pf[0strid
	ride + 0] * pf[2*stride + 1];	e + 0] * pf[2*stride + 1];
	pfres2[2stride + 2] = pf[0stride + 0] * pf[1stride + 1] - pf[0st	pfres2[2stride + 2] = pf[0stride + 0] * pf[1stride + 1] - pf[0strid
	ride + 1] * pf[1*stride + 0];	e + 1] * pf[1*stride + 0];


	T fdet = pf[0stride + 2] pfres2[2stride + 0] + pf[1stride + 2]	T fdet = pf[0stride + 2] pfres2[2stride + 0] + pf[1stride + 2] * p
	* pfres2[2*stride + 1] +	fres2[2*stride + 1] +
	pf[2stride + 2] pfres2[2*stride + 2];	pf[2stride + 2] pfres2[2*stride + 2];


	if( pfdet != NULL )	if( pfdet != NULL )
	pfdet[0] = fdet;	pfdet[0] = fdet;

	if( fabs(fdet) < 1e-12 ) {	if( fabs(fdet) < 1e-12 ) {
	return NULL;	return NULL;
	}	}


	fdet = 1 / fdet;	fdet = 1 / fdet;
	//if( pfdet != NULL ) *pfdet = fdet;	//if( pfdet != NULL ) *pfdet = fdet;


	if( pfres != pf ) {	if( pfres != pf ) {
	pfres[0stride+0] = fdet; pfres[0stride+1] =	pfres[0stride+0] = fdet; pfres[0stride+1] = fdet;
	fdet; pfres[0stride+2] = fdet;	pfres[0stride+2] = fdet;
	pfres[1stride+0] = fdet; pfres[1stride+1] =	pfres[1stride+0] = fdet; pfres[1stride+1] = fdet;
	fdet; pfres[1stride+2] = fdet;	pfres[1stride+2] = fdet;
	pfres[2stride+0] = fdet; pfres[2stride+1] =	pfres[2stride+0] = fdet; pfres[2stride+1] = fdet;
	fdet; pfres[2stride+2] = fdet;	pfres[2stride+2] = fdet;
	return pfres;	return pfres;
	}	}


	pfres[0stride+0] = pfres2[0stride+0] * fdet;	pfres[0stride+0] = pfres2[0stride+0] * fdet;
	pfres[0stride+1] = pfres2[0stride+1] * fdet;	pfres[0stride+1] = pfres2[0stride+1] * fdet;
	pfres[0stride+2] = pfres2[0stride+2] * fdet;	pfres[0stride+2] = pfres2[0stride+2] * fdet;
	pfres[1stride+0] = pfres2[1stride+0] * fdet;	pfres[1stride+0] = pfres2[1stride+0] * fdet;
	pfres[1stride+1] = pfres2[1stride+1] * fdet;	pfres[1stride+1] = pfres2[1stride+1] * fdet;
	pfres[1stride+2] = pfres2[1stride+2] * fdet;	pfres[1stride+2] = pfres2[1stride+2] * fdet;
	pfres[2stride+0] = pfres2[2stride+0] * fdet;	pfres[2stride+0] = pfres2[2stride+0] * fdet;
	pfres[2stride+1] = pfres2[2stride+1] * fdet;	pfres[2stride+1] = pfres2[2stride+1] * fdet;
	pfres[2stride+2] = pfres2[2stride+2] * fdet;	pfres[2stride+2] = pfres2[2stride+2] * fdet;
	return pfres;	return pfres;
	}	}

	/// \brief 4x4 matrix inverse.	/// \brief 4x4 matrix inverse.
	template <typename T>	template <typename T>
	inline T* _inv4(const T* pf, T* pfres)	inline T* _inv4(const T* pf, T* pfres)
	{	{

	T* pfres2;	T* pfres2;
	if( pfres == pf ) pfres2 = (T)alloca(16 sizeof(T));	if( pfres == pf ) pfres2 = (T)alloca(16 sizeof(T));
	else pfres2 = pfres;	else pfres2 = pfres;


	// inverse = C^t / det(pf) where C is the matrix of coefficients	// inverse = C^t / det(pf) where C is the matrix of coefficients


	// calc C^t	// calc C^t


	// determinants of all possibel 2x2 submatrices formed by last two r	// determinants of all possibel 2x2 submatrices formed by last two rows
	ows	T fd0, fd1, fd2;
	T fd0, fd1, fd2;	T f1, f2, f3;
	T f1, f2, f3;	fd0 = pf[24 + 0] pf[34 + 1] - pf[24 + 1] * pf[3*4 + 0];
	fd0 = pf[24 + 0] pf[34 + 1] - pf[24 + 1] * pf[3*4 + 0];	fd1 = pf[24 + 1] pf[34 + 2] - pf[24 + 2] * pf[3*4 + 1];
	fd1 = pf[24 + 1] pf[34 + 2] - pf[24 + 2] * pf[3*4 + 1];	fd2 = pf[24 + 2] pf[34 + 3] - pf[24 + 3] * pf[3*4 + 2];
	fd2 = pf[24 + 2] pf[34 + 3] - pf[24 + 3] * pf[3*4 + 2];


	f1 = pf[24 + 1] pf[34 + 3] - pf[24 + 3] * pf[3*4 + 1];	f1 = pf[24 + 1] pf[34 + 3] - pf[24 + 3] * pf[3*4 + 1];
	f2 = pf[24 + 0] pf[34 + 3] - pf[24 + 3] * pf[3*4 + 0];	f2 = pf[24 + 0] pf[34 + 3] - pf[24 + 3] * pf[3*4 + 0];
	f3 = pf[24 + 0] pf[34 + 2] - pf[24 + 2] * pf[3*4 + 0];	f3 = pf[24 + 0] pf[34 + 2] - pf[24 + 2] * pf[3*4 + 0];


	pfres2[04 + 0] = pf[14 + 1] * fd2 - pf[14 + 2] f1 + pf[1*4 +	pfres2[04 + 0] = pf[14 + 1] * fd2 - pf[14 + 2] f1 + pf[1*4 + 3]
	3] * fd1;	* fd1;
	pfres2[04 + 1] = -(pf[04 + 1] * fd2 - pf[04 + 2] f1 + pf[0*4 +	pfres2[04 + 1] = -(pf[04 + 1] * fd2 - pf[04 + 2] f1 + pf[0*4 + 3]
	3] * fd1);	* fd1);


	pfres2[14 + 0] = -(pf[14 + 0] * fd2 - pf[14 + 2] f2 + pf[1*4 +	pfres2[14 + 0] = -(pf[14 + 0] * fd2 - pf[14 + 2] f2 + pf[1*4 + 3]
	3] * f3);	* f3);
	pfres2[14 + 1] = pf[04 + 0] * fd2 - pf[04 + 2] f2 + pf[0*4 +	pfres2[14 + 1] = pf[04 + 0] * fd2 - pf[04 + 2] f2 + pf[0*4 + 3]
	3] * f3;	* f3;


	pfres2[24 + 0] = pf[14 + 0] * f1 - pf[14 + 1] f2 + pf[1*4 +	pfres2[24 + 0] = pf[14 + 0] * f1 - pf[14 + 1] f2 + pf[1*4 + 3]
	3] * fd0;	* fd0;
	pfres2[24 + 1] = -(pf[04 + 0] * f1 - pf[04 + 1] f2 + pf[0*4 +	pfres2[24 + 1] = -(pf[04 + 0] * f1 - pf[04 + 1] f2 + pf[0*4 + 3]
	3] * fd0);	* fd0);


	pfres2[34 + 0] = -(pf[14 + 0] * fd1 - pf[14 + 1] f3 + pf[1*4 +	pfres2[34 + 0] = -(pf[14 + 0] * fd1 - pf[14 + 1] f3 + pf[1*4 + 2]
	2] * fd0);	* fd0);
	pfres2[34 + 1] = pf[04 + 0] * fd1 - pf[04 + 1] f3 + pf[0*4 +	pfres2[34 + 1] = pf[04 + 0] * fd1 - pf[04 + 1] f3 + pf[0*4 + 2]
	2] * fd0;	* fd0;


	// determinants of first 2 rows of 4x4 matrix	// determinants of first 2 rows of 4x4 matrix
	fd0 = pf[04 + 0] pf[14 + 1] - pf[04 + 1] * pf[1*4 + 0];	fd0 = pf[04 + 0] pf[14 + 1] - pf[04 + 1] * pf[1*4 + 0];
	fd1 = pf[04 + 1] pf[14 + 2] - pf[04 + 2] * pf[1*4 + 1];	fd1 = pf[04 + 1] pf[14 + 2] - pf[04 + 2] * pf[1*4 + 1];
	fd2 = pf[04 + 2] pf[14 + 3] - pf[04 + 3] * pf[1*4 + 2];	fd2 = pf[04 + 2] pf[14 + 3] - pf[04 + 3] * pf[1*4 + 2];


	f1 = pf[04 + 1] pf[14 + 3] - pf[04 + 3] * pf[1*4 + 1];	f1 = pf[04 + 1] pf[14 + 3] - pf[04 + 3] * pf[1*4 + 1];
	f2 = pf[04 + 0] pf[14 + 3] - pf[04 + 3] * pf[1*4 + 0];	f2 = pf[04 + 0] pf[14 + 3] - pf[04 + 3] * pf[1*4 + 0];
	f3 = pf[04 + 0] pf[14 + 2] - pf[04 + 2] * pf[1*4 + 0];	f3 = pf[04 + 0] pf[14 + 2] - pf[04 + 2] * pf[1*4 + 0];


	pfres2[04 + 2] = pf[34 + 1] * fd2 - pf[34 + 2] f1 + pf[3*4 +	pfres2[04 + 2] = pf[34 + 1] * fd2 - pf[34 + 2] f1 + pf[3*4 + 3]
	3] * fd1;	* fd1;
	pfres2[04 + 3] = -(pf[24 + 1] * fd2 - pf[24 + 2] f1 + pf[2*4 +	pfres2[04 + 3] = -(pf[24 + 1] * fd2 - pf[24 + 2] f1 + pf[2*4 + 3]
	3] * fd1);	* fd1);


	pfres2[14 + 2] = -(pf[34 + 0] * fd2 - pf[34 + 2] f2 + pf[3*4 +	pfres2[14 + 2] = -(pf[34 + 0] * fd2 - pf[34 + 2] f2 + pf[3*4 + 3]
	3] * f3);	* f3);
	pfres2[14 + 3] = pf[24 + 0] * fd2 - pf[24 + 2] f2 + pf[2*4 +	pfres2[14 + 3] = pf[24 + 0] * fd2 - pf[24 + 2] f2 + pf[2*4 + 3]
	3] * f3;	* f3;


	pfres2[24 + 2] = pf[34 + 0] * f1 - pf[34 + 1] f2 + pf[3*4 +	pfres2[24 + 2] = pf[34 + 0] * f1 - pf[34 + 1] f2 + pf[3*4 + 3]
	3] * fd0;	* fd0;
	pfres2[24 + 3] = -(pf[24 + 0] * f1 - pf[24 + 1] f2 + pf[2*4 +	pfres2[24 + 3] = -(pf[24 + 0] * f1 - pf[24 + 1] f2 + pf[2*4 + 3]
	3] * fd0);	* fd0);


	pfres2[34 + 2] = -(pf[34 + 0] * fd1 - pf[34 + 1] f3 + pf[3*4 +	pfres2[34 + 2] = -(pf[34 + 0] * fd1 - pf[34 + 1] f3 + pf[3*4 + 2]
	2] * fd0);	* fd0);
	pfres2[34 + 3] = pf[24 + 0] * fd1 - pf[24 + 1] f3 + pf[2*4 +	pfres2[34 + 3] = pf[24 + 0] * fd1 - pf[24 + 1] f3 + pf[2*4 + 2]
	2] * fd0;	* fd0;


	T fdet = pf[04 + 3] pfres2[34 + 0] + pf[14 + 3] * pfres2[3*4 +	T fdet = pf[04 + 3] pfres2[34 + 0] + pf[14 + 3] * pfres2[3*4 + 1]
	1] +	+
	pf[24 + 3] pfres2[34 + 2] + pf[34 + 3] * pfres2	pf[24 + 3] pfres2[34 + 2] + pf[34 + 3] * pfres2[3*4 + 3];
	[3*4 + 3];


	if( fabs(fdet) < 1e-9) return NULL;	if( fabs(fdet) < 1e-9) return NULL;


	fdet = 1 / fdet;	fdet = 1 / fdet;
	//if( pfdet != NULL ) *pfdet = fdet;	//if( pfdet != NULL ) *pfdet = fdet;


	if( pfres2 == pfres ) {	if( pfres2 == pfres ) {
	mult(pfres, fdet, 16);	mult(pfres, fdet, 16);
	return pfres;	return pfres;
	}	}


	int i = 0;	int i = 0;
	while(i < 16) {	while(i < 16) {
	pfres[i] = pfres2[i] * fdet;	pfres[i] = pfres2[i] * fdet;
	++i;	++i;
	}	}


	return pfres;	return pfres;
	}	}

	/// \brief Transpose a 3x3 matrix.	/// \brief Transpose a 3x3 matrix.
	template <typename T>	template <typename T>
	inline T* _transpose3(const T* pf, T* pfres)	inline T* _transpose3(const T* pf, T* pfres)
	{	{

	MATH_ASSERT( pf != NULL && pfres != NULL );	MATH_ASSERT( pf != NULL && pfres != NULL );


	if( pf == pfres ) {	if( pf == pfres ) {
	std::swap(pfres[1], pfres[3]);	std::swap(pfres[1], pfres[3]);

	std::swap(pfres[2], pfres[6]);	std::swap(pfres[2], pfres[6]);
	std::swap(pfres[5], pfres[7]);	std::swap(pfres[5], pfres[7]);
	return pfres;	return pfres;
	}	}


	pfres[0] = pf[0]; pfres[1] = pf[3]; pfres[2] = pf[6];	pfres[0] = pf[0]; pfres[1] = pf[3]; pfres[2] = pf[6];
	pfres[3] = pf[1]; pfres[4] = pf[4]; pfres[5] = pf[7];	pfres[3] = pf[1]; pfres[4] = pf[4]; pfres[5] = pf[7];
	pfres[6] = pf[2]; pfres[7] = pf[5]; pfres[8] = pf[8];	pfres[6] = pf[2]; pfres[7] = pf[5]; pfres[8] = pf[8];


	return pfres;	return pfres;
	}	}

	/// \brief Transpose a 4x4 matrix.	/// \brief Transpose a 4x4 matrix.
	template <typename T>	template <typename T>
	inline T* _transpose4(const T* pf, T* pfres)	inline T* _transpose4(const T* pf, T* pfres)
	{	{

	MATH_ASSERT( pf != NULL && pfres != NULL );	MATH_ASSERT( pf != NULL && pfres != NULL );


	if( pf == pfres ) {	if( pf == pfres ) {
	std::swap(pfres[1], pfres[4]);	std::swap(pfres[1], pfres[4]);
	std::swap(pfres[2], pfres[8]);	std::swap(pfres[2], pfres[8]);
	std::swap(pfres[3], pfres[12]);	std::swap(pfres[3], pfres[12]);
	std::swap(pfres[6], pfres[9]);	std::swap(pfres[6], pfres[9]);
	std::swap(pfres[7], pfres[13]);	std::swap(pfres[7], pfres[13]);
	std::swap(pfres[11], pfres[15]);	std::swap(pfres[11], pfres[15]);
	return pfres;	return pfres;
	}	}


	pfres[0] = pf[0]; pfres[1] = pf[4]; pfres[2] = pf[8];	pfres[0] = pf[0]; pfres[1] = pf[4]; pfres[2] = pf[8];
	pfres[3] = pf[12];	pfres[3] = pf[12];
	pfres[4] = pf[1]; pfres[5] = pf[5]; pfres[6] = pf[9];	pfres[4] = pf[1]; pfres[5] = pf[5]; pfres[6] = pf[9];
	pfres[7] = pf[13];	pfres[7] = pf[13];
	pfres[8] = pf[2]; pfres[9] = pf[6]; pfres[10] = pf[10];	pfres[8] = pf[2]; pfres[9] = pf[6]; pfres[10] = pf[10];
	pfres[11] = pf[14];	pfres[11] = pf[14];
	pfres[12] = pf[3]; pfres[13] = pf[7]; pfres[14] = pf[11];	pfres[12] = pf[3]; pfres[13] = pf[7]; pfres[14] = pf[11];
	pfres[15] = pf[15];	pfres[15] = pf[15];
	return pfres;	return pfres;
	}	}

	template <typename T>	template <typename T>
	inline T _dot2(const T* pf1, const T* pf2)	inline T _dot2(const T* pf1, const T* pf2)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL );
	return pf1[0]pf2[0] + pf1[1]pf2[1];	return pf1[0]pf2[0] + pf1[1]pf2[1];
	}	}

	template <typename T>	template <typename T>
	inline T _dot3(const T* pf1, const T* pf2)	inline T _dot3(const T* pf1, const T* pf2)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL );
	return pf1[0]pf2[0] + pf1[1]pf2[1] + pf1[2]*pf2[2];	return pf1[0]pf2[0] + pf1[1]pf2[1] + pf1[2]*pf2[2];
	}	}

	template <typename T>	template <typename T>
	inline T _dot4(const T* pf1, const T* pf2)	inline T _dot4(const T* pf1, const T* pf2)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL );
	return pf1[0]pf2[0] + pf1[1]pf2[1] + pf1[2]pf2[2] + pf1[3] pf2[	return pf1[0]pf2[0] + pf1[1]pf2[1] + pf1[2]pf2[2] + pf1[3] pf2[3];
	3];
	}	}

	template <typename T>	template <typename T>
	inline T _lengthsqr2(const T* pf)	inline T _lengthsqr2(const T* pf)
	{	{

	MATH_ASSERT( pf != NULL );	MATH_ASSERT( pf != NULL );
	return pf[0] * pf[0] + pf[1] * pf[1];	return pf[0] * pf[0] + pf[1] * pf[1];
	}	}

	template <typename T>	template <typename T>
	inline T _lengthsqr3(const T* pf)	inline T _lengthsqr3(const T* pf)
	{	{

	MATH_ASSERT( pf != NULL );	MATH_ASSERT( pf != NULL );
	return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2];	return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2];
	}	}

	template <typename T>	template <typename T>
	inline T _lengthsqr4(const T* pf)	inline T _lengthsqr4(const T* pf)
	{	{

	MATH_ASSERT( pf != NULL );	MATH_ASSERT( pf != NULL );
	return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2] + pf[3] * pf[3]	return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2] + pf[3] * pf[3];
	;
	}	}

	template <typename T>	template <typename T>
	inline T* _normalize2(T* pfout, const T* pf)	inline T* _normalize2(T* pfout, const T* pf)
	{	{

	MATH_ASSERT(pf != NULL);	MATH_ASSERT(pf != NULL);


	T f = pf[0]pf[0] + pf[1]pf[1];	T f = pf[0]pf[0] + pf[1]pf[1];
	f = 1 / sqrt(f);	f = 1 / sqrt(f);
	pfout[0] = pf[0] * f;	pfout[0] = pf[0] * f;
	pfout[1] = pf[1] * f;	pfout[1] = pf[1] * f;


	return pfout;	return pfout;
	}	}

	template <typename T>	template <typename T>
	inline T* _normalize3(T* pfout, const T* pf)	inline T* _normalize3(T* pfout, const T* pf)
	{	{

	MATH_ASSERT(pf != NULL);	MATH_ASSERT(pf != NULL);


	T f = pf[0]pf[0] + pf[1]pf[1] + pf[2]*pf[2];	T f = pf[0]pf[0] + pf[1]pf[1] + pf[2]*pf[2];


	f = 1 / sqrt(f);	f = 1 / sqrt(f);
	pfout[0] = pf[0] * f;	pfout[0] = pf[0] * f;
	pfout[1] = pf[1] * f;	pfout[1] = pf[1] * f;
	pfout[2] = pf[2] * f;	pfout[2] = pf[2] * f;


	return pfout;	return pfout;
	}	}

	template <typename T>	template <typename T>
	inline T* _normalize4(T* pfout, const T* pf)	inline T* _normalize4(T* pfout, const T* pf)
	{	{

	MATH_ASSERT(pf != NULL);	MATH_ASSERT(pf != NULL);


	T f = pf[0]pf[0] + pf[1]pf[1] + pf[2]pf[2] + pf[3]pf[3];	T f = pf[0]pf[0] + pf[1]pf[1] + pf[2]pf[2] + pf[3]pf[3];


	f = 1 / sqrt(f);	f = 1 / sqrt(f);
	pfout[0] = pf[0] * f;	pfout[0] = pf[0] * f;
	pfout[1] = pf[1] * f;	pfout[1] = pf[1] * f;
	pfout[2] = pf[2] * f;	pfout[2] = pf[2] * f;
	pfout[3] = pf[3] * f;	pfout[3] = pf[3] * f;


	return pfout;	return pfout;
	}	}

	template <typename T>	template <typename T>
	inline T* _cross3(T* pfout, const T* pf1, const T* pf2)	inline T* _cross3(T* pfout, const T* pf1, const T* pf2)
	{	{

	MATH_ASSERT( pfout != NULL && pf1 != NULL && pf2 != NULL );	MATH_ASSERT( pfout != NULL && pf1 != NULL && pf2 != NULL );

	T temp[3];	T temp[3];

	temp[0] = pf1[1] * pf2[2] - pf1[2] * pf2[1];	temp[0] = pf1[1] * pf2[2] - pf1[2] * pf2[1];
	temp[1] = pf1[2] * pf2[0] - pf1[0] * pf2[2];	temp[1] = pf1[2] * pf2[0] - pf1[0] * pf2[2];
	temp[2] = pf1[0] * pf2[1] - pf1[1] * pf2[0];	temp[2] = pf1[0] * pf2[1] - pf1[1] * pf2[0];


	pfout[0] = temp[0]; pfout[1] = temp[1]; pfout[2] = temp[2];	pfout[0] = temp[0]; pfout[1] = temp[1]; pfout[2] = temp[2];
	return pfout;	return pfout;
	}	}

	template <typename T>	template <typename T>
	inline T* _transnorm3(T* pfout, const T* pfmat, const T* pf)	inline T* _transnorm3(T* pfout, const T* pfmat, const T* pf)
	{	{

	MATH_ASSERT( pfout != NULL && pf != NULL && pfmat != NULL );	MATH_ASSERT( pfout != NULL && pf != NULL && pfmat != NULL );

	T dummy[3];	T dummy[3];

	T* pfreal = (pfout == pf) ? dummy : pfout;	T* pfreal = (pfout == pf) ? dummy : pfout;


	pfreal[0] = pf[0] * pfmat[0] + pf[1] * pfmat[1] + pf[2] * pfmat[2];	pfreal[0] = pf[0] * pfmat[0] + pf[1] * pfmat[1] + pf[2] * pfmat[2];
	pfreal[1] = pf[0] * pfmat[3] + pf[1] * pfmat[4] + pf[2] * pfmat[5];	pfreal[1] = pf[0] * pfmat[3] + pf[1] * pfmat[4] + pf[2] * pfmat[5];
	pfreal[2] = pf[0] * pfmat[6] + pf[1] * pfmat[7] + pf[2] * pfmat[8];	pfreal[2] = pf[0] * pfmat[6] + pf[1] * pfmat[7] + pf[2] * pfmat[8];

	if( pfout ==pf ) {	if( pfout ==pf ) {
	pfout[0] = pfreal[0];	pfout[0] = pfreal[0];
	pfout[1] = pfreal[1];	pfout[1] = pfreal[1];
	pfout[2] = pfreal[2];	pfout[2] = pfreal[2];
	}	}


	return pfout;	return pfout;
	}	}


	inline float* mult4(float* pfres, const float* pf1, const float* pf2) { ret	inline float* mult4(float* pfres, const float* pf1, const float* pf2) {
	urn _mult4<float>(pfres, pf1, pf2); }	return _mult4<float>(pfres, pf1, pf2);
		}
	// pf1^T * pf2	// pf1^T * pf2

	inline float* multtrans3(float* pfres, const float* pf1, const float* pf2)	inline float* multtrans3(float* pfres, const float* pf1, const float* pf2)
	{ return _multtrans3<float>(pfres, pf1, pf2); }	{
	inline float* multtrans4(float* pfres, const float* pf1, const float* pf2)	return _multtrans3<float>(pfres, pf1, pf2);
	{ return _multtrans4<float>(pfres, pf1, pf2); }	}
	inline float* transnorm3(float* pfout, const float* pfmat, const float* pf)	inline float* multtrans4(float* pfres, const float* pf1, const float* pf2)
	{ return _transnorm3<float>(pfout, pfmat, pf); }	{
		return _multtrans4<float>(pfres, pf1, pf2);
		}
		inline float* transnorm3(float* pfout, const float* pfmat, const float* pf)
		{
		return _transnorm3<float>(pfout, pfmat, pf);
		}


	inline float* transpose3(const float* pf, float* pfres) { return _transpose	inline float* transpose3(const float* pf, float* pfres) {
	3<float>(pf, pfres); }	return _transpose3<float>(pf, pfres);
	inline float* transpose4(const float* pf, float* pfres) { return _transpose	}
	4<float>(pf, pfres); }	inline float* transpose4(const float* pf, float* pfres) {
		return _transpose4<float>(pf, pfres);
		}


	inline float dot2(const float* pf1, const float* pf2) { return _dot2<float>	inline float dot2(const float* pf1, const float* pf2) {
	(pf1, pf2); }	return _dot2<float>(pf1, pf2);
	inline float dot3(const float* pf1, const float* pf2) { return _dot3<float>	}
	(pf1, pf2); }	inline float dot3(const float* pf1, const float* pf2) {
	inline float dot4(const float* pf1, const float* pf2) { return _dot4<float>	return _dot3<float>(pf1, pf2);
	(pf1, pf2); }	}
		inline float dot4(const float* pf1, const float* pf2) {
		return _dot4<float>(pf1, pf2);
		}


	inline float lengthsqr2(const float* pf) { return _lengthsqr2<float>(pf); }	inline float lengthsqr2(const float* pf) {
	inline float lengthsqr3(const float* pf) { return _lengthsqr3<float>(pf); }	return _lengthsqr2<float>(pf);
	inline float lengthsqr4(const float* pf) { return _lengthsqr4<float>(pf); }	}
		inline float lengthsqr3(const float* pf) {
		return _lengthsqr3<float>(pf);
		}
		inline float lengthsqr4(const float* pf) {
		return _lengthsqr4<float>(pf);
		}


	inline float* normalize2(float* pfout, const float* pf) { return _normalize	inline float* normalize2(float* pfout, const float* pf) {
	2<float>(pfout, pf); }	return _normalize2<float>(pfout, pf);
	inline float* normalize3(float* pfout, const float* pf) { return _normalize	}
	3<float>(pfout, pf); }	inline float* normalize3(float* pfout, const float* pf) {
	inline float* normalize4(float* pfout, const float* pf) { return _normalize	return _normalize3<float>(pfout, pf);
	4<float>(pfout, pf); }	}
		inline float* normalize4(float* pfout, const float* pf) {
		return _normalize4<float>(pfout, pf);
		}


	inline float* cross3(float* pfout, const float* pf1, const float* pf2) { re	inline float* cross3(float* pfout, const float* pf1, const float* pf2) {
	turn _cross3<float>(pfout, pf1, pf2); }	return _cross3<float>(pfout, pf1, pf2);
		}

	// multiplies 3x3 matrices	// multiplies 3x3 matrices

	inline float* mult3_s4(float* pfres, const float* pf1, const float* pf2) {	inline float* mult3_s4(float* pfres, const float* pf1, const float* pf2) {
	return _mult3_s4<float>(pfres, pf1, pf2); }	return _mult3_s4<float>(pfres, pf1, pf2);
	inline float* mult3_s3(float* pfres, const float* pf1, const float* pf2) {	}
	return _mult3_s3<float>(pfres, pf1, pf2); }	inline float* mult3_s3(float* pfres, const float* pf1, const float* pf2) {
		return _mult3_s3<float>(pfres, pf1, pf2);
		}


	inline float* inv3(const float* pf, float* pfres, float* pfdet, int stride)	inline float* inv3(const float* pf, float* pfres, float* pfdet, int stride)
	{ return _inv3<float>(pf, pfres, pfdet, stride); }	{
	inline float* inv4(const float* pf, float* pfres) { return _inv4<float>(pf,	return _inv3<float>(pf, pfres, pfdet, stride);
	pfres); }	}
		inline float* inv4(const float* pf, float* pfres) {
		return _inv4<float>(pf, pfres);
		}


	inline double* mult4(double* pfres, const double* pf1, const double* pf2) {	inline double* mult4(double* pfres, const double* pf1, const double* pf2) {
	return _mult4<double>(pfres, pf1, pf2); }	return _mult4<double>(pfres, pf1, pf2);
		}
	// pf1^T * pf2	// pf1^T * pf2

	inline double* multtrans3(double* pfres, const double* pf1, const double* p	inline double* multtrans3(double* pfres, const double* pf1, const double* p
	f2) { return _multtrans3<double>(pfres, pf1, pf2); }	f2) {
	inline double* multtrans4(double* pfres, const double* pf1, const double* p	return _multtrans3<double>(pfres, pf1, pf2);
	f2) { return _multtrans4<double>(pfres, pf1, pf2); }	}
	inline double* transnorm3(double* pfout, const double* pfmat, const double*	inline double* multtrans4(double* pfres, const double* pf1, const double* p
	pf) { return _transnorm3<double>(pfout, pfmat, pf); }	f2) {
		return _multtrans4<double>(pfres, pf1, pf2);
		}
		inline double* transnorm3(double* pfout, const double* pfmat, const double*
		pf) {
		return _transnorm3<double>(pfout, pfmat, pf);
		}


	inline double* transpose3(const double* pf, double* pfres) { return _transp	inline double* transpose3(const double* pf, double* pfres) {
	ose3<double>(pf, pfres); }	return _transpose3<double>(pf, pfres);
	inline double* transpose4(const double* pf, double* pfres) { return _transp	}
	ose4<double>(pf, pfres); }	inline double* transpose4(const double* pf, double* pfres) {
		return _transpose4<double>(pf, pfres);
		}


	inline double dot2(const double* pf1, const double* pf2) { return _dot2<dou	inline double dot2(const double* pf1, const double* pf2) {
	ble>(pf1, pf2); }	return _dot2<double>(pf1, pf2);
	inline double dot3(const double* pf1, const double* pf2) { return _dot3<dou	}
	ble>(pf1, pf2); }	inline double dot3(const double* pf1, const double* pf2) {
	inline double dot4(const double* pf1, const double* pf2) { return _dot4<dou	return _dot3<double>(pf1, pf2);
	ble>(pf1, pf2); }	}
		inline double dot4(const double* pf1, const double* pf2) {
		return _dot4<double>(pf1, pf2);
		}


	inline double lengthsqr2(const double* pf) { return _lengthsqr2<double>(pf)	inline double lengthsqr2(const double* pf) {
	; }	return _lengthsqr2<double>(pf);
	inline double lengthsqr3(const double* pf) { return _lengthsqr3<double>(pf)	}
	; }	inline double lengthsqr3(const double* pf) {
	inline double lengthsqr4(const double* pf) { return _lengthsqr4<double>(pf)	return _lengthsqr3<double>(pf);
	; }	}
		inline double lengthsqr4(const double* pf) {
		return _lengthsqr4<double>(pf);
		}


	inline double* normalize2(double* pfout, const double* pf) { return _normal	inline double* normalize2(double* pfout, const double* pf) {
	ize2<double>(pfout, pf); }	return _normalize2<double>(pfout, pf);
	inline double* normalize3(double* pfout, const double* pf) { return _normal	}
	ize3<double>(pfout, pf); }	inline double* normalize3(double* pfout, const double* pf) {
	inline double* normalize4(double* pfout, const double* pf) { return _normal	return _normalize3<double>(pfout, pf);
	ize4<double>(pfout, pf); }	}
		inline double* normalize4(double* pfout, const double* pf) {
		return _normalize4<double>(pfout, pf);
		}


	inline double* cross3(double* pfout, const double* pf1, const double* pf2)	inline double* cross3(double* pfout, const double* pf1, const double* pf2)
	{ return _cross3<double>(pfout, pf1, pf2); }	{
		return _cross3<double>(pfout, pf1, pf2);
		}

	// multiplies 3x3 matrices	// multiplies 3x3 matrices

	inline double* mult3_s4(double* pfres, const double* pf1, const double* pf2	inline double* mult3_s4(double* pfres, const double* pf1, const double* pf2
	) { return _mult3_s4<double>(pfres, pf1, pf2); }	) {
	inline double* mult3_s3(double* pfres, const double* pf1, const double* pf2	return _mult3_s4<double>(pfres, pf1, pf2);
	) { return _mult3_s3<double>(pfres, pf1, pf2); }	}
		inline double* mult3_s3(double* pfres, const double* pf1, const double* pf2
		) {
		return _mult3_s3<double>(pfres, pf1, pf2);
		}


	inline double* inv3(const double* pf, double* pfres, double* pfdet, int str	inline double* inv3(const double* pf, double* pfres, double* pfdet, int str
	ide) { return _inv3<double>(pf, pfres, pfdet, stride); }	ide) {
	inline double* inv4(const double* pf, double* pfres) { return _inv4<double>	return _inv3<double>(pf, pfres, pfdet, stride);
	(pf, pfres); }	}
		inline double* inv4(const double* pf, double* pfres) {
		return _inv4<double>(pf, pfres);
		}

	template <typename T, typename R, typename S>	template <typename T, typename R, typename S>
	inline S* mult(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)	inline S* mult(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);
	int j, k;	int j, k;


	if( !badd ) memset(pfres, 0, sizeof(S) * r1 * c2);	if( !badd ) memset(pfres, 0, sizeof(S) * r1 * c2);


	while(r1 > 0) {	while(r1 > 0) {
	--r1;	--r1;


	j = 0;	j = 0;
	while(j < c2) {	while(j < c2) {
	k = 0;	k = 0;
	while(k < c1) {	while(k < c1) {
	pfres[j] += (S)(pf1[k] * pf2[k*c2 + j]);	pfres[j] += (S)(pf1[k] * pf2[k*c2 + j]);
	++k;	++k;
	}	}
	++j;	++j;
	}	}


	pf1 += c1;	pf1 += c1;
	pfres += c2;	pfres += c2;
	}	}


	return pfres;	return pfres;
	}	}

	template <typename T, typename R, typename S>	template <typename T, typename R, typename S>
	inline S* multtrans(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)	inline S* multtrans(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);
	int i, j, k;	int i, j, k;


	if( !badd ) memset(pfres, 0, sizeof(S) * c1 * c2);	if( !badd ) memset(pfres, 0, sizeof(S) * c1 * c2);


	i = 0;	i = 0;
	while(i < c1) {	while(i < c1) {


	j = 0;	j = 0;
	while(j < c2) {	while(j < c2) {


	k = 0;	k = 0;
	while(k < r1) {	while(k < r1) {
	pfres[j] += (S)(pf1[kc1] pf2[k*c2 + j]);	pfres[j] += (S)(pf1[kc1] pf2[k*c2 + j]);
	++k;	++k;
	}	}
	++j;	++j;
	}	}


	pfres += c2;	pfres += c2;
	++pf1;	++pf1;


	++i;	++i;
	}	}


	return pfres;	return pfres;
	}	}

	template <typename T, typename R, typename S>	template <typename T, typename R, typename S>
	inline S* multtrans_to2(T* pf1, R* pf2, int r1, int c1, int r2, S* pfres, b ool badd)	inline S* multtrans_to2(T* pf1, R* pf2, int r1, int c1, int r2, S* pfres, b ool badd)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);	MATH_ASSERT( pf1 != NULL && pf2 != NULL && pfres != NULL);
	int j, k;	int j, k;


	if( !badd ) memset(pfres, 0, sizeof(S) * r1 * r2);	if( !badd ) memset(pfres, 0, sizeof(S) * r1 * r2);


	while(r1 > 0) {	while(r1 > 0) {
	--r1;	--r1;


	j = 0;	j = 0;
	while(j < r2) {	while(j < r2) {
	k = 0;	k = 0;
	while(k < c1) {	while(k < c1) {
	pfres[j] += (S)(pf1[k] * pf2[j*c1 + k]);	pfres[j] += (S)(pf1[k] * pf2[j*c1 + k]);
	++k;	++k;
	}	}
	++j;	++j;
	}	}


	pf1 += c1;	pf1 += c1;
	pfres += r2;	pfres += r2;

	}	}


	return pfres;	return pfres;
	}	}

	template <typename T> inline T* multto1(T* pf1, T* pf2, int r, int c, T* pf temp)	template <typename T> inline T* multto1(T* pf1, T* pf2, int r, int c, T* pf temp)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL );


	int j, k;	int j, k;
	bool bdel = false;	bool bdel = false;


	if( pftemp == NULL ) {	if( pftemp == NULL ) {
	pftemp = new T[c];	pftemp = new T[c];
	bdel = true;	bdel = true;
	}	}


	while(r > 0) {	while(r > 0) {
	--r;	--r;


	j = 0;	j = 0;
	while(j < c) {	while(j < c) {


	pftemp[j] = 0.0;	pftemp[j] = 0.0;


	k = 0;	k = 0;
	while(k < c) {	while(k < c) {
	pftemp[j] += pf1[k] * pf2[k*c + j];	pftemp[j] += pf1[k] * pf2[k*c + j];
	++k;	++k;
	}	}
	++j;	++j;
	}	}


	memcpy(pf1, pftemp, c * sizeof(T));	memcpy(pf1, pftemp, c * sizeof(T));
	pf1 += c;	pf1 += c;
	}	}


	if( bdel ) delete[] pftemp;	if( bdel ) delete[] pftemp;


	return pf1;	return pf1;
	}	}

	template <typename T, typename S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp)	template <typename T, typename S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL );	MATH_ASSERT( pf1 != NULL && pf2 != NULL );


	int i, j, k;	int i, j, k;
	bool bdel = false;	bool bdel = false;


	if( pftemp == NULL ) {	if( pftemp == NULL ) {
	pftemp = new S[r2];	pftemp = new S[r2];
	bdel = true;	bdel = true;
	}	}


	// do columns first	// do columns first
	j = 0;	j = 0;
	while(j < c2) {	while(j < c2) {
	i = 0;	i = 0;
	while(i < r2) {	while(i < r2) {


	pftemp[i] = 0.0;	pftemp[i] = 0.0;


	k = 0;	k = 0;
	while(k < r2) {	while(k < r2) {
	pftemp[i] += pf1[ir2 + k] pf2[k*c2 + j];	pftemp[i] += pf1[ir2 + k] pf2[k*c2 + j];
	++k;	++k;
	}	}
	++i;	++i;
	}	}


	i = 0;	i = 0;
	while(i < r2) {	while(i < r2) {
	(pf2+ic2+j) = pftemp[i];	(pf2+ic2+j) = pftemp[i];
	++i;	++i;
	}	}


	++j;	++j;
	}	}


	if( bdel ) delete[] pftemp;	if( bdel ) delete[] pftemp;


	return pf1;	return pf1;
	}	}

	template <typename T> inline void add(T* pf1, T* pf2, int r)	template <typename T> inline void add(T* pf1, T* pf2, int r)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL);	MATH_ASSERT( pf1 != NULL && pf2 != NULL);


	while(r > 0) {	while(r > 0) {
	--r;	--r;
	pf1[r] += pf2[r];	pf1[r] += pf2[r];
	}	}
	}	}

	template <typename T> inline void sub(T* pf1, T* pf2, int r)	template <typename T> inline void sub(T* pf1, T* pf2, int r)
	{	{

	MATH_ASSERT( pf1 != NULL && pf2 != NULL);	MATH_ASSERT( pf1 != NULL && pf2 != NULL);


	while(r > 0) {	while(r > 0) {
	--r;	--r;
	pf1[r] -= pf2[r];	pf1[r] -= pf2[r];
	}	}
	}	}

	template <typename T> inline T normsqr(const T* pf1, int r)	template <typename T> inline T normsqr(const T* pf1, int r)
	{	{

	MATH_ASSERT( pf1 != NULL );	MATH_ASSERT( pf1 != NULL );


	T d = 0.0;	T d = 0.0;
	while(r > 0) {	while(r > 0) {
	--r;	--r;
	d += pf1[r] * pf1[r];	d += pf1[r] * pf1[r];
	}	}


	return d;	return d;
	}	}

	template <typename T> inline T lengthsqr(const T* pf1, const T* pf2, int le ngth)	template <typename T> inline T lengthsqr(const T* pf1, const T* pf2, int le ngth)
	{	{

	T d = 0;	T d = 0;
	while(length > 0) {	while(length > 0) {
	--length;	--length;
	T t = pf1[length] - pf2[length];	T t = pf1[length] - pf2[length];

	d += t * t;	d += t * t;
	}	}


	return d;	return d;
	}	}

	template <typename T> inline T dot(T* pf1, T* pf2, int length)	template <typename T> inline T dot(T* pf1, T* pf2, int length)
	{	{

	T d = 0;	T d = 0;
	while(length > 0) {	while(length > 0) {
	--length;	--length;
	d += pf1[length] * pf2[length];	d += pf1[length] * pf2[length];
	}	}


	return d;	return d;
	}	}

	template <typename T> inline T sum(T* pf, int length)	template <typename T> inline T sum(T* pf, int length)
	{	{

	T d = 0;	T d = 0;
	while(length > 0) {	while(length > 0) {
	--length;	--length;
	d += pf[length];	d += pf[length];
	}	}


	return d;	return d;
	}	}

	template <typename T> inline bool inv2(T* pf, T* pfres)	template <typename T> inline bool inv2(T* pf, T* pfres)
	{	{

	T fdet = pf[0] * pf[3] - pf[1] * pf[2];	T fdet = pf[0] * pf[3] - pf[1] * pf[2];


	if( fabs(fdet) < 1e-16 ) return false;	if( fabs(fdet) < 1e-16 ) return false;


	fdet = 1 / fdet;	fdet = 1 / fdet;
	//if( pfdet != NULL ) *pfdet = fdet;	//if( pfdet != NULL ) *pfdet = fdet;


	if( pfres != pf ) {	if( pfres != pf ) {
	pfres[0] = fdet * pf[3]; pfres[1] = -fdet * p	pfres[0] = fdet * pf[3]; pfres[1] = -fdet * pf[1];
	f[1];	pfres[2] = -fdet * pf[2]; pfres[3] = fdet * pf[0];
	pfres[2] = -fdet * pf[2]; pfres[3] = fdet * pf	return true;
	[0];	}
	return true;
	}


	T ftemp = pf[0];	T ftemp = pf[0];
	pfres[0] = pf[3] * fdet;	pfres[0] = pf[3] * fdet;
	pfres[1] *= -fdet;	pfres[1] *= -fdet;
	pfres[2] *= -fdet;	pfres[2] *= -fdet;
	pfres[3] = ftemp * pf[0];	pfres[3] = ftemp * pf[0];


	return true;	return true;
	}	}

	template <typename T, typename S>	template <typename T, typename S>
	void Tridiagonal3 (S* mat, T* diag, T* subd)	void Tridiagonal3 (S* mat, T* diag, T* subd)
	{	{
	T a, b, c, d, e, f, ell, q;	T a, b, c, d, e, f, ell, q;

	a = mat[0*3+0];	a = mat[0*3+0];
	b = mat[0*3+1];	b = mat[0*3+1];
	c = mat[0*3+2];	c = mat[0*3+2];
	d = mat[1*3+1];	d = mat[1*3+1];
	e = mat[1*3+2];	e = mat[1*3+2];
	f = mat[2*3+2];	f = mat[2*3+2];


	subd[2] = 0.0;	subd[2] = 0.0;
	diag[0] = a;	diag[0] = a;
	if ( fabs(c) >= g_fEpsilon ) {	if ( fabs(c) >= g_fEpsilon ) {
	ell = (T)sqrt(bb+cc);	ell = (T)sqrt(bb+cc);
	b /= ell;	b /= ell;
	c /= ell;	c /= ell;
	q = 2be+c*(f-d);	q = 2be+c*(f-d);
	diag[1] = d+c*q;	diag[1] = d+c*q;
	diag[2] = f-c*q;	diag[2] = f-c*q;
	subd[0] = ell;	subd[0] = ell;
	subd[1] = e-b*q;	subd[1] = e-b*q;

End of changes. 150 change blocks.
	471 lines changed or deleted	517 lines changed or added

	openrave.h	openrave.h

	skipping to change at line 19	skipping to change at line 19
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	// GNU Lesser General Public License for more details.	// GNU Lesser General Public License for more details.
	//	//
	// You should have received a copy of the GNU Lesser General Public License	// You should have received a copy of the GNU Lesser General Public License
	// along with this program. If not, see <http://www.gnu.org/licenses/>.	// along with this program. If not, see <http://www.gnu.org/licenses/>.
	/** \file openrave.h	/** \file openrave.h
	\brief Defines the public headers that every plugin must include in or der to use openrave properly.	\brief Defines the public headers that every plugin must include in or der to use openrave properly.

	*/	*/
	#ifndef OPENRAVE_H	#ifndef OPENRAVE_H
	#define OPENRAVE_H	#define OPENRAVE_H

	#ifndef RAVE_DISABLE_ASSERT_HANDLER	#ifndef RAVE_DISABLE_ASSERT_HANDLER
	#define BOOST_ENABLE_ASSERT_HANDLER	#define BOOST_ENABLE_ASSERT_HANDLER
	#endif	#endif

	#include <cstdio>	#include <cstdio>
	#include <stdarg.h>	#include <stdarg.h>
	#include <cstring>	#include <cstring>

	skipping to change at line 154	skipping to change at line 154
	ORE_InvalidPlugin=5, ///< shared object is not a valid plugin	ORE_InvalidPlugin=5, ///< shared object is not a valid plugin
	ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins	ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins
	ORE_NotImplemented=7, ///< function is not implemented by the interface .	ORE_NotImplemented=7, ///< function is not implemented by the interface .
	ORE_InconsistentConstraints=8, ///< return solutions or trajectories do not follow the constraints of the planner/module	ORE_InconsistentConstraints=8, ///< return solutions or trajectories do not follow the constraints of the planner/module
	};	};

	/// \brief Exception that all OpenRAVE internal methods throw; the error co des are held in \ref OpenRAVEErrorCode.	/// \brief Exception that all OpenRAVE internal methods throw; the error co des are held in \ref OpenRAVEErrorCode.
	class OPENRAVE_API openrave_exception : public std::exception	class OPENRAVE_API openrave_exception : public std::exception
	{	{
	public:	public:

	openrave_exception() : std::exception(), _s("unknown exception"), _erro	openrave_exception() : std::exception(), _s("unknown exception"), _erro
	r(ORE_Failed) {}	r(ORE_Failed) {
		}
	openrave_exception(const std::string& s, OpenRAVEErrorCode error=ORE_Fa iled) : std::exception() {	openrave_exception(const std::string& s, OpenRAVEErrorCode error=ORE_Fa iled) : std::exception() {
	_error = error;	_error = error;
	_s = "openrave (";	_s = "openrave (";
	switch(_error) {	switch(_error) {
	case ORE_Failed: _s += "Failed"; break;	case ORE_Failed: _s += "Failed"; break;
	case ORE_InvalidArguments: _s += "InvalidArguments"; break;	case ORE_InvalidArguments: _s += "InvalidArguments"; break;
	case ORE_EnvironmentNotLocked: _s += "EnvironmentNotLocked"; break;	case ORE_EnvironmentNotLocked: _s += "EnvironmentNotLocked"; break;
	case ORE_CommandNotSupported: _s += "CommandNotSupported"; break;	case ORE_CommandNotSupported: _s += "CommandNotSupported"; break;
	case ORE_Assert: _s += "Assert"; break;	case ORE_Assert: _s += "Assert"; break;
	case ORE_InvalidPlugin: _s += "InvalidPlugin"; break;	case ORE_InvalidPlugin: _s += "InvalidPlugin"; break;
	case ORE_InvalidInterfaceHash: _s += "InvalidInterfaceHash"; break;	case ORE_InvalidInterfaceHash: _s += "InvalidInterfaceHash"; break;
	case ORE_NotImplemented: _s += "NotImplemented"; break;	case ORE_NotImplemented: _s += "NotImplemented"; break;
	default:	default:
	_s += boost::str(boost::format("%8.8x")%static_cast<int>(_error ));	_s += boost::str(boost::format("%8.8x")%static_cast<int>(_error ));
	break;	break;
	}	}
	_s += "): ";	_s += "): ";
	_s += s;	_s += s;
	}	}

	virtual ~openrave_exception() throw() {}	virtual ~openrave_exception() throw() {
	char const* what() const throw() { return _s.c_str(); }	}
	const std::string& message() const { return _s; }	char const* what() const throw() {
	OpenRAVEErrorCode GetCode() const { return _error; }	return _s.c_str();
		}
		const std::string& message() const {
		return _s;
		}
		OpenRAVEErrorCode GetCode() const {
		return _error;
		}
	private:	private:
	std::string _s;	std::string _s;
	OpenRAVEErrorCode _error;	OpenRAVEErrorCode _error;
	};	};

	class OPENRAVE_LOCAL CaseInsensitiveCompare	class OPENRAVE_LOCAL CaseInsensitiveCompare
	{	{
	public:	public:

	bool operator()(const std::string & s1, const std::string& s2) const	bool operator() (const std::string & s1, const std::string& s2) const
	{	{
	std::string::const_iterator it1=s1.begin();	std::string::const_iterator it1=s1.begin();
	std::string::const_iterator it2=s2.begin();	std::string::const_iterator it2=s2.begin();

	//has the end of at least one of the strings been reached?	//has the end of at least one of the strings been reached?
	while ( (it1!=s1.end()) && (it2!=s2.end()) ) {	while ( (it1!=s1.end()) && (it2!=s2.end()) ) {

	if(::toupper(it1) != ::toupper(it2)) { //letters differ?	if(::toupper(it1) != ::toupper(it2)) { //letters differ?
	// return -1 to indicate 'smaller than', 1 otherwise	// return -1 to indicate 'smaller than', 1 otherwise
	return ::toupper(it1) < ::toupper(it2);	return ::toupper(it1) < ::toupper(it2);
	}	}
	//proceed to the next character in each string	//proceed to the next character in each string
	++it1;	++it1;
	++it2;	++it2;
	}	}

	std::size_t size1=s1.size(), size2=s2.size();// cache lengths	std::size_t size1=s1.size(), size2=s2.size(); // cache lengths
	//return -1,0 or 1 according to strings' lengths	//return -1,0 or 1 according to strings' lengths
	if (size1==size2) {	if (size1==size2) {
	return 0;	return 0;
	}	}
	return size1<size2;	return size1<size2;
	}	}
	};	};

	/// \brief base class for all user data	/// \brief base class for all user data
	class OPENRAVE_API UserData	class OPENRAVE_API UserData
	{	{

	public:	public:
	virtual ~UserData() {}	virtual ~UserData() {
		}
	};	};
	typedef boost::shared_ptr<UserData> UserDataPtr;	typedef boost::shared_ptr<UserData> UserDataPtr;

	// terminal attributes	// terminal attributes
	//#define RESET 0	//#define RESET 0
	//#define BRIGHT 1	//#define BRIGHT 1
	//#define DIM 2	//#define DIM 2
	//#define UNDERLINE 3	//#define UNDERLINE 3
	//#define BLINK 4	//#define BLINK 4
	//#define REVERSE 7	//#define REVERSE 7

	skipping to change at line 337	skipping to change at line 346
	const char* p = p0 > p1 ? p0 : p1;	const char* p = p0 > p1 ? p0 : p1;
	if( p == NULL ) {	if( p == NULL ) {
	return pfilename;	return pfilename;
	}	}
	return p+1;	return p+1;
	}	}

	#ifdef _WIN32	#ifdef _WIN32

	#define DefineRavePrintfW(LEVEL) \	#define DefineRavePrintfW(LEVEL) \

	inline int RavePrintfW##LEVEL(const wchar_t *fmt, ...) \	inline int RavePrintfW ## LEVEL(const wchar_t *fmt, ...) \
	{ \	{ \
	/ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);/ \	/ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);/ \
	va_list list; \	va_list list; \

	va_start(list,fmt); \	va_start(list,fmt); \
	int r = vwprintf(OpenRAVE::RavePrintTransformString(fmt).c_str(), l ist); \	int r = vwprintf(OpenRAVE::RavePrintTransformString(fmt).c_str(), l ist); \
	va_end(list); \	va_end(list); \
	/ResetTextColor (stdout);/ \	/ResetTextColor (stdout);/ \
	return r; \	return r; \
	}	}

	#define DefineRavePrintfA(LEVEL) \	#define DefineRavePrintfA(LEVEL) \

	inline int RavePrintfA##LEVEL(const std::string& s) \	inline int RavePrintfA ## LEVEL(const std::string& s) \
	{ \	{ \

	if( s.size() == 0 \|\| s[s.size()-1] != '\n' ) { \	if((s.size() == 0)\|\|(s[s.size()-1] != '\n')) { \
	printf("%s\n", s.c_str()); \	printf("%s\n", s.c_str()); \
	} \	} \
	else { \	else { \
	printf ("%s", s.c_str()); \	printf ("%s", s.c_str()); \
	} \	} \
	return s.size(); \	return s.size(); \
	} \	} \
	\	\

	inline int RavePrintfA##LEVEL(const char *fmt, ...) \	inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
	{ \	{ \
	/ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);/ \	/ChangeTextColor (stdout, 0, OPENRAVECOLOR##LEVEL);/ \
	va_list list; \	va_list list; \
	va_start(list,fmt); \	va_start(list,fmt); \
	int r = vprintf(fmt, list); \	int r = vprintf(fmt, list); \
	va_end(list); \	va_end(list); \
	/if( fmt[0] != '\n' ) { printf("\n"); }/ \	/if( fmt[0] != '\n' ) { printf("\n"); }/ \
	/ResetTextColor(stdout);/ \	/ResetTextColor(stdout);/ \
	return r; \	return r; \
	}	}

	inline int RavePrintfA(const std::string& s, uint32_t level)	inline int RavePrintfA(const std::string& s, uint32_t level)
	{	{

	if( s.size() == 0 \|\| s[s.size()-1] != '\n' ) { // automatically add a n ew line	if((s.size() == 0)\|\|(s[s.size()-1] != '\n')) { // automatically add a n ew line
	printf("%s\n", s.c_str());	printf("%s\n", s.c_str());
	}	}
	else {	else {
	printf ("%s", s.c_str());	printf ("%s", s.c_str());
	}	}
	return s.size();	return s.size();
	}	}

	DefineRavePrintfW(_INFOLEVEL)	DefineRavePrintfW(_INFOLEVEL)
	DefineRavePrintfA(_INFOLEVEL)	DefineRavePrintfA(_INFOLEVEL)

	#else	#else

	#define DefineRavePrintfW(LEVEL) \	#define DefineRavePrintfW(LEVEL) \

	inline int RavePrintfW##LEVEL(const wchar_t *wfmt, ...) \	inline int RavePrintfW ## LEVEL(const wchar_t *wfmt, ...) \
	{ \	{ \
	va_list list; \	va_list list; \
	va_start(list,wfmt); \	va_start(list,wfmt); \
	/* Allocate memory on the stack to avoid heap fragmentation */ \	/* Allocate memory on the stack to avoid heap fragmentation */ \
	size_t allocsize = wcstombs(NULL, wfmt, 0)+32; \	size_t allocsize = wcstombs(NULL, wfmt, 0)+32; \
	char* fmt = (char*)alloca(allocsize); \	char* fmt = (char*)alloca(allocsize); \

	strcpy(fmt, ChangeTextColor(0, OPENRAVECOLOR##LEVEL,8).c_str()); \	strcpy(fmt, ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8).c_str()); \
	snprintf(fmt+strlen(fmt),allocsize-16,"%S",wfmt); \	snprintf(fmt+strlen(fmt),allocsize-16,"%S",wfmt); \
	strcat(fmt, ResetTextColor().c_str()); \	strcat(fmt, ResetTextColor().c_str()); \
	int r = vprintf(fmt, list); \	int r = vprintf(fmt, list); \
	va_end(list); \	va_end(list); \
	return r; \	return r; \
	}	}

	// In linux, only wprintf will succeed, due to the fwide() call in main, so	// In linux, only wprintf will succeed, due to the fwide() call in main, so
	// for programmers who want to use regular format strings without	// for programmers who want to use regular format strings without
	// the L in front, we will take their regular string and widen it	// the L in front, we will take their regular string and widen it
	// for them.	// for them.

	inline int RavePrintfA_INFOLEVEL(const std::string& s)	inline int RavePrintfA_INFOLEVEL(const std::string& s)
	{	{
	if( s.size() == 0 \|\| s[s.size()-1] != '\n' ) { // automatically add	if((s.size() == 0)\|\|(s[s.size()-1] != '\n')) { // automatically add
	a new line	a new line
	printf("%s\n", s.c_str());	printf("%s\n", s.c_str());
	}
	else {
	printf ("%s", s.c_str());
	}
	return s.size();
	}	}

		else {
	inline int RavePrintfA_INFOLEVEL(const char *fmt, ...)	printf ("%s", s.c_str());
	{
	va_list list;
	va_start(list,fmt);
	int r = vprintf(fmt, list);
	va_end(list);
	//if( fmt[0] != '\n' ) { printf("\n"); }
	return r;
	}	}

		return s.size();
		}

		inline int RavePrintfA_INFOLEVEL(const char *fmt, ...)
		{
		va_list list;
		va_start(list,fmt);
		int r = vprintf(fmt, list);
		va_end(list);
		//if( fmt[0] != '\n' ) { printf("\n"); }
		return r;
		}

	#define DefineRavePrintfA(LEVEL) \	#define DefineRavePrintfA(LEVEL) \

	inline int RavePrintfA##LEVEL(const std::string& s) \	inline int RavePrintfA ## LEVEL(const std::string& s) \
	{ \	{ \

	if( s.size() == 0 \|\| s[s.size()-1] != '\n' ) { \	if((s.size() == 0)\|\|(s[s.size()-1] != '\n')) { \
	printf ("%c[0;%d;%dm%s%c[m\n", 0x1B, OPENRAVECOLOR##LEVEL + 30,	printf ("%c[0;%d;%dm%s%c[m\n", 0x1B, OPENRAVECOLOR ## LEVEL + 3
	8+40,s.c_str(),0x1B); \	0,8+40,s.c_str(),0x1B); \
	} \	} \
	else { \	else { \

	printf ("%c[0;%d;%dm%s%c[m", 0x1B, OPENRAVECOLOR##LEVEL + 30,8+ 40,s.c_str(),0x1B); \	printf ("%c[0;%d;%dm%s%c[m", 0x1B, OPENRAVECOLOR ## LEVEL + 30, 8+40,s.c_str(),0x1B); \
	} \	} \
	return s.size(); \	return s.size(); \
	} \	} \
	\	\

	inline int RavePrintfA##LEVEL(const char *fmt, ...) \	inline int RavePrintfA ## LEVEL(const char *fmt, ...) \
	{ \	{ \
	va_list list; \	va_list list; \

	va_start(list,fmt); \	va_start(list,fmt); \
	int r = vprintf((ChangeTextColor(0, OPENRAVECOLOR##LEVEL,8) + std::	int r = vprintf((ChangeTextColor(0, OPENRAVECOLOR ## LEVEL,8) + std
	string(fmt) + ResetTextColor()).c_str(), list); \	::string(fmt) + ResetTextColor()).c_str(), list); \
	va_end(list); \	va_end(list); \
	/if( fmt[0] != '\n' ) { printf("\n"); } / \	/if( fmt[0] != '\n' ) { printf("\n"); } / \
	return r; \	return r; \
	} \	} \

	inline int RavePrintfA(const std::string& s, uint32_t level)	inline int RavePrintfA(const std::string& s, uint32_t level)
	{	{
	if( (OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=level ) {	if( (OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Level_OutputMask)>=level ) {
	int color = 0;	int color = 0;
	switch(level) {	switch(level) {
	case Level_Fatal: color = OPENRAVECOLOR_FATALLEVEL; break;	case Level_Fatal: color = OPENRAVECOLOR_FATALLEVEL; break;
	case Level_Error: color = OPENRAVECOLOR_ERRORLEVEL; break;	case Level_Error: color = OPENRAVECOLOR_ERRORLEVEL; break;
	case Level_Warn: color = OPENRAVECOLOR_WARNLEVEL; break;	case Level_Warn: color = OPENRAVECOLOR_WARNLEVEL; break;
	case Level_Info: // print regular	case Level_Info: // print regular

	if( s.size() == 0 \|\| s[s.size()-1] != '\n' ) { // automatically add a new line	if((s.size() == 0)\|\|(s[s.size()-1] != '\n')) { // automatically add a new line
	printf ("%s\n",s.c_str());	printf ("%s\n",s.c_str());
	}	}
	else {	else {
	printf ("%s",s.c_str());	printf ("%s",s.c_str());
	}	}
	return s.size();	return s.size();
	case Level_Debug: color = OPENRAVECOLOR_DEBUGLEVEL; break;	case Level_Debug: color = OPENRAVECOLOR_DEBUGLEVEL; break;
	case Level_Verbose: color = OPENRAVECOLOR_VERBOSELEVEL; break;	case Level_Verbose: color = OPENRAVECOLOR_VERBOSELEVEL; break;
	}	}

	if( s.size() == 0 \|\| s[s.size()-1] != '\n' ) { // automatically add a new line	if((s.size() == 0)\|\|(s[s.size()-1] != '\n')) { // automatically add a new line
	printf ("%c[0;%d;%dm%s%c[0;38;48m\n", 0x1B, color + 30,8+40,s.c _str(),0x1B);	printf ("%c[0;%d;%dm%s%c[0;38;48m\n", 0x1B, color + 30,8+40,s.c _str(),0x1B);
	}	}
	else {	else {
	printf ("%c[0;%d;%dm%s%c[0;38;48m", 0x1B, color + 30,8+40,s.c_s tr(),0x1B);	printf ("%c[0;%d;%dm%s%c[0;38;48m", 0x1B, color + 30,8+40,s.c_s tr(),0x1B);
	}	}
	return s.size();	return s.size();
	}	}
	return 0;	return 0;
	}	}


	skipping to change at line 497	skipping to change at line 506
	DefineRavePrintfW(_DEBUGLEVEL)	DefineRavePrintfW(_DEBUGLEVEL)
	DefineRavePrintfW(_VERBOSELEVEL)	DefineRavePrintfW(_VERBOSELEVEL)

	DefineRavePrintfA(_FATALLEVEL)	DefineRavePrintfA(_FATALLEVEL)
	DefineRavePrintfA(_ERRORLEVEL)	DefineRavePrintfA(_ERRORLEVEL)
	DefineRavePrintfA(_WARNLEVEL)	DefineRavePrintfA(_WARNLEVEL)
	//DefineRavePrintfA(_INFOLEVEL)	//DefineRavePrintfA(_INFOLEVEL)
	DefineRavePrintfA(_DEBUGLEVEL)	DefineRavePrintfA(_DEBUGLEVEL)
	DefineRavePrintfA(_VERBOSELEVEL)	DefineRavePrintfA(_VERBOSELEVEL)


	#define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA##LEVEL("[%s:%d] ", Ope nRAVE::RaveGetSourceFilename(__FILE__), __LINE__)	#define RAVEPRINTHEADER(LEVEL) OpenRAVE::RavePrintfA ## LEVEL("[%s:%d] ", O penRAVE::RaveGetSourceFilename(__FILE__), __LINE__)

	// different logging levels. The higher the suffix number, the less importa nt the information is.	// different logging levels. The higher the suffix number, the less importa nt the information is.
	// 0 log level logs all the time. OpenRAVE starts up with a log level of 0.	// 0 log level logs all the time. OpenRAVE starts up with a log level of 0.

	#define RAVELOG_LEVELW(LEVEL,level) (OpenRAVE::RaveGetDebugLevel()&OpenRAVE	#define RAVELOG_LEVELW(LEVEL,level) (OpenRAVE::RaveGetDebugLevel()&OpenRAVE
	::Level_OutputMask)>=(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePri	::Level_OutputMask)>=(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePri
	ntfW##LEVEL	ntfW ## LEVEL
	#define RAVELOG_LEVELA(LEVEL,level) (OpenRAVE::RaveGetDebugLevel()&OpenRAVE	#define RAVELOG_LEVELA(LEVEL,level) (OpenRAVE::RaveGetDebugLevel()&OpenRAVE
	::Level_OutputMask)>=(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePri	::Level_OutputMask)>=(level)&&(RAVEPRINTHEADER(LEVEL)>0)&&OpenRAVE::RavePri
	ntfA##LEVEL	ntfA ## LEVEL

	// define log4cxx equivalents (eventually OpenRAVE will move to log4cxx log ging)	// define log4cxx equivalents (eventually OpenRAVE will move to log4cxx log ging)
	#define RAVELOG_FATALW RAVELOG_LEVELW(_FATALLEVEL,OpenRAVE::Level_Fatal)	#define RAVELOG_FATALW RAVELOG_LEVELW(_FATALLEVEL,OpenRAVE::Level_Fatal)
	#define RAVELOG_FATALA RAVELOG_LEVELA(_FATALLEVEL,OpenRAVE::Level_Fatal)	#define RAVELOG_FATALA RAVELOG_LEVELA(_FATALLEVEL,OpenRAVE::Level_Fatal)
	#define RAVELOG_FATAL RAVELOG_FATALA	#define RAVELOG_FATAL RAVELOG_FATALA
	#define RAVELOG_ERRORW RAVELOG_LEVELW(_ERRORLEVEL,OpenRAVE::Level_Error)	#define RAVELOG_ERRORW RAVELOG_LEVELW(_ERRORLEVEL,OpenRAVE::Level_Error)
	#define RAVELOG_ERRORA RAVELOG_LEVELA(_ERRORLEVEL,OpenRAVE::Level_Error)	#define RAVELOG_ERRORA RAVELOG_LEVELA(_ERRORLEVEL,OpenRAVE::Level_Error)
	#define RAVELOG_ERROR RAVELOG_ERRORA	#define RAVELOG_ERROR RAVELOG_ERRORA
	#define RAVELOG_WARNW RAVELOG_LEVELW(_WARNLEVEL,OpenRAVE::Level_Warn)	#define RAVELOG_WARNW RAVELOG_LEVELW(_WARNLEVEL,OpenRAVE::Level_Warn)
	#define RAVELOG_WARNA RAVELOG_LEVELA(_WARNLEVEL,OpenRAVE::Level_Warn)	#define RAVELOG_WARNA RAVELOG_LEVELA(_WARNLEVEL,OpenRAVE::Level_Warn)

	skipping to change at line 526	skipping to change at line 535
	#define RAVELOG_INFO RAVELOG_INFOA	#define RAVELOG_INFO RAVELOG_INFOA
	#define RAVELOG_DEBUGW RAVELOG_LEVELW(_DEBUGLEVEL,OpenRAVE::Level_Debug)	#define RAVELOG_DEBUGW RAVELOG_LEVELW(_DEBUGLEVEL,OpenRAVE::Level_Debug)
	#define RAVELOG_DEBUGA RAVELOG_LEVELA(_DEBUGLEVEL,OpenRAVE::Level_Debug)	#define RAVELOG_DEBUGA RAVELOG_LEVELA(_DEBUGLEVEL,OpenRAVE::Level_Debug)
	#define RAVELOG_DEBUG RAVELOG_DEBUGA	#define RAVELOG_DEBUG RAVELOG_DEBUGA
	#define RAVELOG_VERBOSEW RAVELOG_LEVELW(_VERBOSELEVEL,OpenRAVE::Level_Verbo se)	#define RAVELOG_VERBOSEW RAVELOG_LEVELW(_VERBOSELEVEL,OpenRAVE::Level_Verbo se)
	#define RAVELOG_VERBOSEA RAVELOG_LEVELA(_VERBOSELEVEL,OpenRAVE::Level_Verbo se)	#define RAVELOG_VERBOSEA RAVELOG_LEVELA(_VERBOSELEVEL,OpenRAVE::Level_Verbo se)
	#define RAVELOG_VERBOSE RAVELOG_VERBOSEA	#define RAVELOG_VERBOSE RAVELOG_VERBOSEA

	#define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Leve l_OutputMask)>=(level))	#define IS_DEBUGLEVEL(level) ((OpenRAVE::RaveGetDebugLevel()&OpenRAVE::Leve l_OutputMask)>=(level))


	#define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_excepti on(boost::str(boost::format("[%s:%d] "s)%(__PRETTY_FUNCTION__)%(__LINE__)), errorcode)	#define OPENRAVE_EXCEPTION_FORMAT0(s, errorcode) OpenRAVE::openrave_excepti on(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE__)) ,errorcode)

	/// adds the function name and line number to an openrave exception	/// adds the function name and line number to an openrave exception

	#define OPENRAVE_EXCEPTION_FORMAT(s, args,errorcode) OpenRAVE::openrave_exc eption(boost::str(boost::format("[%s:%d] "s)%(__PRETTY_FUNCTION__)%(__LINE_ _)%args),errorcode)	#define OPENRAVE_EXCEPTION_FORMAT(s, args,errorcode) OpenRAVE::openrave_exc eption(boost::str(boost::format("[%s:%d] " s)%(__PRETTY_FUNCTION__)%(__LINE __)%args),errorcode)

	#define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("n ot implemented",ORE_NotImplemented); }	#define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("n ot implemented",ORE_NotImplemented); }

	/// \brief Enumeration of all the interfaces.	/// \brief Enumeration of all the interfaces.
	enum InterfaceType	enum InterfaceType
	{	{
	PT_Planner=1, ///< describes \ref PlannerBase interface	PT_Planner=1, ///< describes \ref PlannerBase interface
	PT_Robot=2, ///< describes \ref RobotBase interface	PT_Robot=2, ///< describes \ref RobotBase interface
	PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface	PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface
	PT_Controller=4, ///< describes \ref ControllerBase interface	PT_Controller=4, ///< describes \ref ControllerBase interface
	PT_Module=5, ///< describes \ref ModuleBase interface	PT_Module=5, ///< describes \ref ModuleBase interface
	PT_ProblemInstance=5, ///< describes \ref ModuleBase interface	PT_ProblemInstance=5, ///< describes \ref ModuleBase interface
	PT_IkSolver=6, ///< describes \ref IkSolverBase interface	PT_IkSolver=6, ///< describes \ref IkSolverBase interface
	PT_InverseKinematicsSolver=6, ///< describes \ref IkSolverBase interfac e	PT_InverseKinematicsSolver=6, ///< describes \ref IkSolverBase interfac e
	PT_KinBody=7, ///< describes \ref KinBody	PT_KinBody=7, ///< describes \ref KinBody
	PT_PhysicsEngine=8, ///< describes \ref PhysicsEngineBase	PT_PhysicsEngine=8, ///< describes \ref PhysicsEngineBase
	PT_Sensor=9, ///< describes \ref SensorBase	PT_Sensor=9, ///< describes \ref SensorBase
	PT_CollisionChecker=10, ///< describes \ref CollisionCheckerBase	PT_CollisionChecker=10, ///< describes \ref CollisionCheckerBase
	PT_Trajectory=11, ///< describes \ref TrajectoryBase	PT_Trajectory=11, ///< describes \ref TrajectoryBase

	PT_Viewer=12,///< describes \ref ViewerBase	PT_Viewer=12, ///< describes \ref ViewerBase
	PT_SpaceSampler=13, ///< describes \ref SamplerBase	PT_SpaceSampler=13, ///< describes \ref SamplerBase
	PT_NumberOfInterfaces=13 ///< number of interfaces, do not forget to up date	PT_NumberOfInterfaces=13 ///< number of interfaces, do not forget to up date
	};	};

	class CollisionReport;	class CollisionReport;
	class InterfaceBase;	class InterfaceBase;
	class IkSolverBase;	class IkSolverBase;
	class TrajectoryBase;	class TrajectoryBase;
	class ControllerBase;	class ControllerBase;
	class PlannerBase;	class PlannerBase;

	skipping to change at line 633	skipping to change at line 642
	Clone_Viewer = 2, ///< clone the viewer type, although figures won't be copied, new viewer does try to match views	Clone_Viewer = 2, ///< clone the viewer type, although figures won't be copied, new viewer does try to match views
	Clone_Simulation = 4, ///< clone the physics engine and simulation stat e (ie, timesteps, gravity)	Clone_Simulation = 4, ///< clone the physics engine and simulation stat e (ie, timesteps, gravity)
	Clone_RealControllers = 8, ///< if specified, will clone the real contr ollers of all the robots, otherwise each robot gets ideal controller	Clone_RealControllers = 8, ///< if specified, will clone the real contr ollers of all the robots, otherwise each robot gets ideal controller
	Clone_Sensors = 16, ///< if specified, will clone the sensors attached to the robot and added to the environment	Clone_Sensors = 16, ///< if specified, will clone the sensors attached to the robot and added to the environment
	};	};

	/// base class for readable interfaces	/// base class for readable interfaces
	class OPENRAVE_API XMLReadable	class OPENRAVE_API XMLReadable
	{	{
	public:	public:

	XMLReadable(const std::string& xmlid) : __xmlid(xmlid) {}	XMLReadable(const std::string& xmlid) : __xmlid(xmlid) {
	virtual ~XMLReadable() {}	}
	virtual const std::string& GetXMLId() const { return __xmlid; }	virtual ~XMLReadable() {
		}
		virtual const std::string& GetXMLId() const {
		return __xmlid;
		}
	private:	private:
	std::string __xmlid;	std::string __xmlid;
	};	};

	typedef boost::shared_ptr<XMLReadable> XMLReadablePtr;	typedef boost::shared_ptr<XMLReadable> XMLReadablePtr;
	typedef boost::shared_ptr<XMLReadable const> XMLReadableConstPtr;	typedef boost::shared_ptr<XMLReadable const> XMLReadableConstPtr;
	typedef std::list<std::pair<std::string,std::string> > AttributesList;	typedef std::list<std::pair<std::string,std::string> > AttributesList;
	/// \deprecated (11/02/18)	/// \deprecated (11/02/18)
	typedef AttributesList XMLAttributesList RAVE_DEPRECATED;	typedef AttributesList XMLAttributesList RAVE_DEPRECATED;

	/// base class for all xml readers. XMLReaders are used to process data fro m	/// base class for all xml readers. XMLReaders are used to process data fro m
	/// xml files. Custom readers can be registered through EnvironmentBase.	/// xml files. Custom readers can be registered through EnvironmentBase.
	/// By default it can record all data that is encountered inside the xml re ader	/// By default it can record all data that is encountered inside the xml re ader
	class OPENRAVE_API BaseXMLReader : public boost::enable_shared_from_this<Ba seXMLReader>	class OPENRAVE_API BaseXMLReader : public boost::enable_shared_from_this<Ba seXMLReader>
	{	{
	public:	public:
	enum ProcessElement	enum ProcessElement
	{	{

	PE_Pass=0, ///< current tag was not supported, so pass onto another	PE_Pass=0, ///< current tag was not supported, so pass onto ano
	class	ther class
	PE_Support=1, ///< current tag will be processed by this class	PE_Support=1, ///< current tag will be processed by this class
	PE_Ignore=2, ///< current tag and all its children should be ignore	PE_Ignore=2, ///< current tag and all its children should be ig
	d	nored
	};	};

	BaseXMLReader() {}	BaseXMLReader() {
	virtual ~BaseXMLReader() {}	}
		virtual ~BaseXMLReader() {
		}

	/// a readable interface that stores the information processsed for the current tag	/// a readable interface that stores the information processsed for the current tag
	/// This pointer is used to the InterfaceBase class registered readers	/// This pointer is used to the InterfaceBase class registered readers

	virtual XMLReadablePtr GetReadable() { return XMLReadablePtr(); }	virtual XMLReadablePtr GetReadable() {
		return XMLReadablePtr();
		}

	/// Gets called in the beginning of each "<type>" expression. In this c ase, name is "type"	/// Gets called in the beginning of each "<type>" expression. In this c ase, name is "type"
	/// \param name of the tag, will be always lower case	/// \param name of the tag, will be always lower case
	/// \param atts string of attributes where the first std::string is the attribute name and second is the value	/// \param atts string of attributes where the first std::string is the attribute name and second is the value
	/// \return true if tag is accepted and this class will process it, oth erwise false	/// \return true if tag is accepted and this class will process it, oth erwise false
	virtual ProcessElement startElement(const std::string& name, const Attr ibutesList& atts) = 0;	virtual ProcessElement startElement(const std::string& name, const Attr ibutesList& atts) = 0;

	/// Gets called at the end of each "</type>" expression. In this case, name is "type"	/// Gets called at the end of each "</type>" expression. In this case, name is "type"
	/// \param name of the tag, will be always lower case	/// \param name of the tag, will be always lower case
	/// \return true if XMLReader has finished parsing (one condition is th at name==_fieldname) , otherwise false	/// \return true if XMLReader has finished parsing (one condition is th at name==_fieldname) , otherwise false

	skipping to change at line 697	skipping to change at line 714
	typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const Attributes List&)> CreateXMLReaderFn;	typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const Attributes List&)> CreateXMLReaderFn;

	/// reads until the tag ends	/// reads until the tag ends
	class OPENRAVE_API DummyXMLReader : public BaseXMLReader	class OPENRAVE_API DummyXMLReader : public BaseXMLReader
	{	{
	public:	public:
	DummyXMLReader(const std::string& pfieldname, const std::string& pparen tname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::os tream>());	DummyXMLReader(const std::string& pfieldname, const std::string& pparen tname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::os tream>());
	virtual ProcessElement startElement(const std::string& name, const Attr ibutesList& atts);	virtual ProcessElement startElement(const std::string& name, const Attr ibutesList& atts);
	virtual bool endElement(const std::string& name);	virtual bool endElement(const std::string& name);
	virtual void characters(const std::string& ch);	virtual void characters(const std::string& ch);

	const std::string& GetFieldName() const { return _fieldname; }	const std::string& GetFieldName() const {
		return _fieldname;
		}
	private:	private:

	std::string _parentname; /// XML filename	std::string _parentname; /// XML filename
	std::string _fieldname;	std::string _fieldname;

	boost::shared_ptr<std::ostream> _osrecord; ///< used to store the xml d ata	boost::shared_ptr<std::ostream> _osrecord; ///< used to store the x ml data
	boost::shared_ptr<BaseXMLReader> _pcurreader;	boost::shared_ptr<BaseXMLReader> _pcurreader;
	};	};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	// define the math functions	// define the math functions
	#define MATH_EXP RaveExp	#define MATH_EXP RaveExp
	#define MATH_LOG RaveLog	#define MATH_LOG RaveLog
	#define MATH_COS RaveCos	#define MATH_COS RaveCos
	#define MATH_SIN RaveSin	#define MATH_SIN RaveSin

	skipping to change at line 726	skipping to change at line 745
	#define MATH_ASIN RaveAsin	#define MATH_ASIN RaveAsin
	#define MATH_ATAN2 RaveAtan2	#define MATH_ATAN2 RaveAtan2
	#define MATH_POW RavePow	#define MATH_POW RavePow
	#define MATH_SQRT RaveSqrt	#define MATH_SQRT RaveSqrt
	#define MATH_FABS RaveFabs	#define MATH_FABS RaveFabs

	#include <openrave/geometry.h>	#include <openrave/geometry.h>
	#include <openrave/mathextra.h>	#include <openrave/mathextra.h>

	namespace OpenRAVE {	namespace OpenRAVE {

	using geometry::RaveVector;	using geometry::RaveVector;
	using geometry::RaveTransform;	using geometry::RaveTransform;
	using geometry::RaveTransformMatrix;	using geometry::RaveTransformMatrix;
	typedef RaveVector<dReal> Vector;	typedef RaveVector<dReal> Vector;
	typedef RaveTransform<dReal> Transform;	typedef RaveTransform<dReal> Transform;
	typedef boost::shared_ptr< RaveTransform<dReal> > TransformPtr;	typedef boost::shared_ptr< RaveTransform<dReal> > TransformPtr;
	typedef boost::shared_ptr< RaveTransform<dReal> const > TransformConstP	typedef boost::shared_ptr< RaveTransform<dReal> const > TransformConstPtr;
	tr;	typedef RaveTransformMatrix<dReal> TransformMatrix;
	typedef RaveTransformMatrix<dReal> TransformMatrix;	typedef boost::shared_ptr< RaveTransformMatrix<dReal> > TransformMatrixPtr;
	typedef boost::shared_ptr< RaveTransformMatrix<dReal> > TransformMatrix	typedef boost::shared_ptr< RaveTransformMatrix<dReal> const > TransformMatr
	Ptr;	ixConstPtr;
	typedef boost::shared_ptr< RaveTransformMatrix<dReal> const > Transform	typedef geometry::obb<dReal> OBB;
	MatrixConstPtr;	typedef geometry::aabb<dReal> AABB;
	typedef geometry::obb<dReal> OBB;	typedef geometry::ray<dReal> RAY;
	typedef geometry::aabb<dReal> AABB;
	typedef geometry::ray<dReal> RAY;


	// for compatibility	// for compatibility
	//@{	//@{
	using mathextra::dot2;	using mathextra::dot2;
	using mathextra::dot3;	using mathextra::dot3;
	using mathextra::dot4;	using mathextra::dot4;
	using mathextra::normalize2;	using mathextra::normalize2;
	using mathextra::normalize3;	using mathextra::normalize3;
	using mathextra::normalize4;	using mathextra::normalize4;
	using mathextra::cross3;	using mathextra::cross3;
	using mathextra::inv3;	using mathextra::inv3;
	using mathextra::inv4;	using mathextra::inv4;
	using mathextra::lengthsqr2;	using mathextra::lengthsqr2;
	using mathextra::lengthsqr3;	using mathextra::lengthsqr3;
	using mathextra::lengthsqr4;	using mathextra::lengthsqr4;
	using mathextra::mult4;	using mathextra::mult4;
	//@}	//@}

	/** \brief Parameterization of basic primitives for querying inverse-kinema tics solutions.	/** \brief Parameterization of basic primitives for querying inverse-kinema tics solutions.

	Holds the parameterization of a geometric primitive useful for autonomo us manipulation scenarios like:	Holds the parameterization of a geometric primitive useful for autonomo us manipulation scenarios like:
	6D pose, 3D translation, 3D rotation, 3D look at direction, and ray loo k at direction.	6D pose, 3D translation, 3D rotation, 3D look at direction, and ray loo k at direction.

	*/	*/
	class OPENRAVE_API IkParameterization	class OPENRAVE_API IkParameterization
	{	{
	public:	public:
	/// \brief The types of inverse kinematics parameterizations supported.	/// \brief The types of inverse kinematics parameterizations supported.
	///	///
	/// The minimum degree of freedoms required is set in the upper 4 bits of each type.	/// The minimum degree of freedoms required is set in the upper 4 bits of each type.
	/// The number of values used to represent the parameterization ( >= do f ) is the next 4 bits.	/// The number of values used to represent the parameterization ( >= do f ) is the next 4 bits.
	/// The lower bits contain a unique id of the type.	/// The lower bits contain a unique id of the type.
	enum Type {	enum Type {
	Type_None=0,	Type_None=0,

	Type_Transform6D=0x67000001, ///< end effector reaches desired 6D t	Type_Transform6D=0x67000001, ///< end effector reaches desired
	ransformation	6D transformation
	Type_Rotation3D=0x34000002, ///< end effector reaches desired 3D ro	Type_Rotation3D=0x34000002, ///< end effector reaches desired 3
	tation	D rotation
	Type_Translation3D=0x33000003, ///< end effector origin reaches des	Type_Translation3D=0x33000003, ///< end effector origin reaches
	ired 3D translation	desired 3D translation
	Type_Direction3D=0x23000004, ///< direction on end effector coordin	Type_Direction3D=0x23000004, ///< direction on end effector coo
	ate system reaches desired direction	rdinate system reaches desired direction
	Type_Ray4D=0x46000005, ///< ray on end effector coordinate system r	Type_Ray4D=0x46000005, ///< ray on end effector coordinate syst
	eaches desired global ray	em reaches desired global ray
	Type_Lookat3D=0x23000006, ///< direction on end effector coordinate	Type_Lookat3D=0x23000006, ///< direction on end effector coordi
	system points to desired 3D position	nate system points to desired 3D position
	Type_TranslationDirection5D=0x56000007, ///< end effector origin an	Type_TranslationDirection5D=0x56000007, ///< end effector origi
	d direction reaches desired 3D translation and direction. Can be thought of	n and direction reaches desired 3D translation and direction. Can be though
	as Ray IK where the origin of the ray must coincide.	t of as Ray IK where the origin of the ray must coincide.
	Type_TranslationXY2D=0x22000008, ///< 2D translation along XY plane	Type_TranslationXY2D=0x22000008, ///< 2D translation along XY p
	Type_TranslationXYOrientation3D=0x33000009, ///< 2D translation alo	lane
	ng XY plane and 1D rotation around Z axis. The offset of the rotation is me	Type_TranslationXYOrientation3D=0x33000009, ///< 2D translation
	asured starting at +X, so at +X is it 0, at +Y it is pi/2.	along XY plane and 1D rotation around Z axis. The offset of the rotation i
	Type_TranslationLocalGlobal6D=0x3600000a, ///< local point on end e	s measured starting at +X, so at +X is it 0, at +Y it is pi/2.
	ffector origin reaches desired 3D global point	Type_TranslationLocalGlobal6D=0x3600000a, ///< local point on e
	Type_NumberOfParameterizations=10, ///< number of parameterizations	nd effector origin reaches desired 3D global point
	(does not count Type_None)	Type_NumberOfParameterizations=10, ///< number of parameterizat
		ions (does not count Type_None)
	};	};


	IkParameterization() : _type(Type_None) {}	IkParameterization() : _type(Type_None) {
		}
	/// \brief sets a 6D transform parameterization	/// \brief sets a 6D transform parameterization

	IkParameterization(const Transform& t) { SetTransform6D(t); }	IkParameterization(const Transform &t) {
		SetTransform6D(t);
		}
	/// \brief sets a ray parameterization	/// \brief sets a ray parameterization

	IkParameterization(const RAY& r) { SetRay4D(r); }	IkParameterization(const RAY &r) {
		SetRay4D(r);
		}
	/// \brief set a custom parameterization using a transform as the sourc e of the data. Not all types are supported with this method.	/// \brief set a custom parameterization using a transform as the sourc e of the data. Not all types are supported with this method.

	IkParameterization(const Transform& t, Type type) {	IkParameterization(const Transform &t, Type type) {
	_type=type;	_type=type;
	switch(_type) {	switch(_type) {
	case Type_Transform6D: SetTransform6D(t); break;	case Type_Transform6D: SetTransform6D(t); break;
	case Type_Rotation3D: SetRotation3D(t.rot); break;	case Type_Rotation3D: SetRotation3D(t.rot); break;
	case Type_Translation3D: SetTranslation3D(t.trans); break;	case Type_Translation3D: SetTranslation3D(t.trans); break;
	case Type_Lookat3D: SetLookat3D(t.trans); break;	case Type_Lookat3D: SetLookat3D(t.trans); break;
	default:	default:
	throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type));	throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type));
	}	}
	}	}


	inline Type GetType() const { return _type; }	inline Type GetType() const {
		return _type;
		}
	inline const std::string& GetName() const;	inline const std::string& GetName() const;

	/// \brief Returns the minimum degree of freedoms required for the IK t ype.	/// \brief Returns the minimum degree of freedoms required for the IK t ype.

	static int GetDOF(Type type) { return (type>>28)&0xf; }	static int GetDOF(Type type) {
		return (type>>28)&0xf;
		}
	/// \brief Returns the minimum degree of freedoms required for the IK t ype.	/// \brief Returns the minimum degree of freedoms required for the IK t ype.

	inline int GetDOF() const { return (_type>>28)&0xf; }	inline int GetDOF() const {
		return (_type>>28)&0xf;
		}

	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.

	static int GetNumberOfValues(Type type) { return (type>>24)&0xf; }	static int GetNumberOfValues(Type type) {
		return (type>>24)&0xf;
		}
	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.

	inline int GetNumberOfValues() const { return (_type>>24)&0xf; }	inline int GetNumberOfValues() const {
		return (_type>>24)&0xf;
		}


	inline void SetTransform6D(const Transform& t) { _type = Type_Transform	inline void SetTransform6D(const Transform& t) {
	6D; _transform = t; }	_type = Type_Transform6D; _transform = t;
	inline void SetRotation3D(const Vector& quaternion) { _type = Type_Rota	}
	tion3D; _transform.rot = quaternion; }	inline void SetRotation3D(const Vector& quaternion) {
	inline void SetTranslation3D(const Vector& trans) { _type = Type_Transl	_type = Type_Rotation3D; _transform.rot = quaternion;
	ation3D; _transform.trans = trans; }	}
	inline void SetDirection3D(const Vector& dir) { _type = Type_Direction3	inline void SetTranslation3D(const Vector& trans) {
	D; _transform.rot = dir; }	_type = Type_Translation3D; _transform.trans = trans;
	inline void SetRay4D(const RAY& ray) { _type = Type_Ray4D; _transform.t	}
	rans = ray.pos; _transform.rot = ray.dir; }	inline void SetDirection3D(const Vector& dir) {
	inline void SetLookat3D(const Vector& trans) { _type = Type_Lookat3D; _	_type = Type_Direction3D; _transform.rot = dir;
	transform.trans = trans; }	}
		inline void SetRay4D(const RAY& ray) {
		_type = Type_Ray4D; _transform.trans = ray.pos; _transform.rot = ra
		y.dir;
		}
		inline void SetLookat3D(const Vector& trans) {
		_type = Type_Lookat3D; _transform.trans = trans;
		}
	/// \brief the ray direction is not used for IK, however it is needed i n order to compute the error	/// \brief the ray direction is not used for IK, however it is needed i n order to compute the error

	inline void SetLookat3D(const RAY& ray) { _type = Type_Lookat3D; _trans	inline void SetLookat3D(const RAY& ray) {
	form.trans = ray.pos; _transform.rot = ray.dir; }	_type = Type_Lookat3D; _transform.trans = ray.pos; _transform.rot =
	inline void SetTranslationDirection5D(const RAY& ray) { _type = Type_Tr	ray.dir;
	anslationDirection5D; _transform.trans = ray.pos; _transform.rot = ray.dir;	}
	}	inline void SetTranslationDirection5D(const RAY& ray) {
	inline void SetTranslationXY2D(const Vector& trans) { _type = Type_Tran	_type = Type_TranslationDirection5D; _transform.trans = ray.pos; _t
	slationXY2D; _transform.trans.x = trans.x; _transform.trans.y = trans.y; _t	ransform.rot = ray.dir;
	ransform.trans.z = 0; _transform.trans.w = 0; }	}
	inline void SetTranslationXYOrientation3D(const Vector& trans) { _type	inline void SetTranslationXY2D(const Vector& trans) {
	= Type_TranslationXYOrientation3D; _transform.trans.x = trans.x; _transform	_type = Type_TranslationXY2D; _transform.trans.x = trans.x; _transf
	.trans.y = trans.y; _transform.trans.z = trans.z; _transform.trans.w = 0; }	orm.trans.y = trans.y; _transform.trans.z = 0; _transform.trans.w = 0;
	inline void SetTranslationLocalGlobal6D(const Vector& localtrans, const	}
	Vector& trans) { _type = Type_TranslationLocalGlobal6D; _transform.rot.x =	inline void SetTranslationXYOrientation3D(const Vector& trans) {
	localtrans.x; _transform.rot.y = localtrans.y; _transform.rot.z = localtra	_type = Type_TranslationXYOrientation3D; _transform.trans.x = trans
	ns.z; _transform.rot.w = 0; _transform.trans.x = trans.x; _transform.trans.	.x; _transform.trans.y = trans.y; _transform.trans.z = trans.z; _transform.
	y = trans.y; _transform.trans.z = trans.z; _transform.trans.w = 0; }	trans.w = 0;
		}
		inline void SetTranslationLocalGlobal6D(const Vector& localtrans, const
		Vector& trans) {
		_type = Type_TranslationLocalGlobal6D; _transform.rot.x = localtran
		s.x; _transform.rot.y = localtrans.y; _transform.rot.z = localtrans.z; _tra
		nsform.rot.w = 0; _transform.trans.x = trans.x; _transform.trans.y = trans.
		y; _transform.trans.z = trans.z; _transform.trans.w = 0;
		}


	inline const Transform& GetTransform6D() const { return _transform; }	inline const Transform& GetTransform6D() const {
	inline const Vector& GetRotation3D() const { return _transform.rot; }	return _transform;
	inline const Vector& GetTranslation3D() const { return _transform.trans	}
	; }	inline const Vector& GetRotation3D() const {
	inline const Vector& GetDirection3D() const { return _transform.rot; }	return _transform.rot;
	inline const RAY GetRay4D() const { return RAY(_transform.trans,_transf	}
	orm.rot); }	inline const Vector& GetTranslation3D() const {
	inline const Vector& GetLookat3D() const { return _transform.trans; }	return _transform.trans;
	inline const Vector& GetLookat3DDirection() const { return _transform.r	}
	ot; }	inline const Vector& GetDirection3D() const {
	inline const RAY GetTranslationDirection5D() const { return RAY(_transf	return _transform.rot;
	orm.trans,_transform.rot); }	}
	inline const Vector& GetTranslationXY2D() const { return _transform.tra	inline const RAY GetRay4D() const {
	ns; }	return RAY(_transform.trans,_transform.rot);
	inline const Vector& GetTranslationXYOrientation3D() const { return _tr	}
	ansform.trans; }	inline const Vector& GetLookat3D() const {
	inline std::pair<Vector,Vector> GetTranslationLocalGlobal6D() const { r	return _transform.trans;
	eturn std::make_pair(_transform.rot,_transform.trans); }	}
		inline const Vector& GetLookat3DDirection() const {
		return _transform.rot;
		}
		inline const RAY GetTranslationDirection5D() const {
		return RAY(_transform.trans,_transform.rot);
		}
		inline const Vector& GetTranslationXY2D() const {
		return _transform.trans;
		}
		inline const Vector& GetTranslationXYOrientation3D() const {
		return _transform.trans;
		}
		inline std::pair<Vector,Vector> GetTranslationLocalGlobal6D() const {
		return std::make_pair(_transform.rot,_transform.trans);
		}

	/// \deprecated (11/02/15)	/// \deprecated (11/02/15)
	//@{	//@{

	inline void SetTransform(const Transform& t) RAVE_DEPRECATED { SetTrans	inline void SetTransform(const Transform& t) RAVE_DEPRECATED {
	form6D(t); }	SetTransform6D(t);
	inline void SetRotation(const Vector& quaternion) RAVE_DEPRECATED { Set	}
	Rotation3D(quaternion); }	inline void SetRotation(const Vector& quaternion) RAVE_DEPRECATED {
	inline void SetTranslation(const Vector& trans) RAVE_DEPRECATED { SetTr	SetRotation3D(quaternion);
	anslation3D(trans); }	}
	inline void SetDirection(const Vector& dir) RAVE_DEPRECATED { SetDirect	inline void SetTranslation(const Vector& trans) RAVE_DEPRECATED {
	ion3D(dir); }	SetTranslation3D(trans);
	inline void SetRay(const RAY& ray) RAVE_DEPRECATED { SetRay4D(ray); }	}
	inline void SetLookat(const Vector& trans) RAVE_DEPRECATED { SetLookat3	inline void SetDirection(const Vector& dir) RAVE_DEPRECATED {
	D(trans); }	SetDirection3D(dir);
	inline void SetTranslationDirection(const RAY& ray) RAVE_DEPRECATED { S	}
	etTranslationDirection5D(ray); }	inline void SetRay(const RAY& ray) RAVE_DEPRECATED {
	inline const Transform& GetTransform() const RAVE_DEPRECATED { return _	SetRay4D(ray);
	transform; }	}
	inline const Vector& GetRotation() const RAVE_DEPRECATED { return _tran	inline void SetLookat(const Vector& trans) RAVE_DEPRECATED {
	sform.rot; }	SetLookat3D(trans);
	inline const Vector& GetTranslation() const RAVE_DEPRECATED { return _t	}
	ransform.trans; }	inline void SetTranslationDirection(const RAY& ray) RAVE_DEPRECATED {
	inline const Vector& GetDirection() const RAVE_DEPRECATED { return _tra	SetTranslationDirection5D(ray);
	nsform.rot; }	}
	inline const Vector& GetLookat() const RAVE_DEPRECATED { return _transf	inline const Transform& GetTransform() const RAVE_DEPRECATED {
	orm.trans; }	return _transform;
	inline const RAY GetRay() const RAVE_DEPRECATED { return RAY(_transform	}
	.trans,_transform.rot); }	inline const Vector& GetRotation() const RAVE_DEPRECATED {
	inline const RAY GetTranslationDirection() const RAVE_DEPRECATED { retu	return _transform.rot;
	rn RAY(_transform.trans,_transform.rot); }	}
		inline const Vector& GetTranslation() const RAVE_DEPRECATED {
		return _transform.trans;
		}
		inline const Vector& GetDirection() const RAVE_DEPRECATED {
		return _transform.rot;
		}
		inline const Vector& GetLookat() const RAVE_DEPRECATED {
		return _transform.trans;
		}
		inline const RAY GetRay() const RAVE_DEPRECATED {
		return RAY(_transform.trans,_transform.rot);
		}
		inline const RAY GetTranslationDirection() const RAVE_DEPRECATED {
		return RAY(_transform.trans,_transform.rot);
		}
	//@}	//@}

	/// \brief Computes the distance squared between two IK parmaeterizatio ns.	/// \brief Computes the distance squared between two IK parmaeterizatio ns.
	inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) cons t	inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) cons t
	{	{

	const dReal anglemult = 0.4; // this is a hack that should be remov ed....	const dReal anglemult = 0.4; // this is a hack that should be r emoved....
	BOOST_ASSERT(_type==ikparam.GetType());	BOOST_ASSERT(_type==ikparam.GetType());
	switch(_type) {	switch(_type) {
	case IkParameterization::Type_Transform6D: {	case IkParameterization::Type_Transform6D: {
	Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D();	Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D();
	dReal fcos = RaveFabs(t0.rot.dot(t1.rot));	dReal fcos = RaveFabs(t0.rot.dot(t1.rot));
	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
	return (t0.trans-t1.trans).lengthsqr3() + anglemultfacosfacos ;	return (t0.trans-t1.trans).lengthsqr3() + anglemultfacosfacos ;
	}	}
	case IkParameterization::Type_Rotation3D: {	case IkParameterization::Type_Rotation3D: {
	dReal fcos = RaveFabs(GetRotation3D().dot(ikparam.GetRotation3D ()));	dReal fcos = RaveFabs(GetRotation3D().dot(ikparam.GetRotation3D ()));

	skipping to change at line 893	skipping to change at line 999
	case IkParameterization::Type_Ray4D: {	case IkParameterization::Type_Ray4D: {
	Vector pos0 = GetRay4D().pos - GetRay4D().dir*GetRay4D().dir.do t(GetRay4D().pos);	Vector pos0 = GetRay4D().pos - GetRay4D().dir*GetRay4D().dir.do t(GetRay4D().pos);
	Vector pos1 = ikparam.GetRay4D().pos - ikparam.GetRay4D().dir*i kparam.GetRay4D().dir.dot(ikparam.GetRay4D().pos);	Vector pos1 = ikparam.GetRay4D().pos - ikparam.GetRay4D().dir*i kparam.GetRay4D().dir.dot(ikparam.GetRay4D().pos);
	dReal fcos = GetRay4D().dir.dot(ikparam.GetRay4D().dir);	dReal fcos = GetRay4D().dir.dot(ikparam.GetRay4D().dir);
	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
	return (pos0-pos1).lengthsqr3() + anglemultfacosfacos;	return (pos0-pos1).lengthsqr3() + anglemultfacosfacos;
	}	}
	case IkParameterization::Type_Lookat3D: {	case IkParameterization::Type_Lookat3D: {
	Vector v = GetLookat3D()-ikparam.GetLookat3D();	Vector v = GetLookat3D()-ikparam.GetLookat3D();
	dReal s = v.dot3(ikparam.GetLookat3DDirection());	dReal s = v.dot3(ikparam.GetLookat3DDirection());

	if( s >= -1 ) { // ikparam's lookat is always 1 beyond the orig in, this is just the convention for testing...	if( s >= -1 ) { // ikparam's lookat is always 1 beyond the origin, this is just the convention for testing...
	v -= s*ikparam.GetLookat3DDirection();	v -= s*ikparam.GetLookat3DDirection();
	}	}
	return v.lengthsqr3();	return v.lengthsqr3();
	}	}
	case IkParameterization::Type_TranslationDirection5D: {	case IkParameterization::Type_TranslationDirection5D: {
	dReal fcos = GetTranslationDirection5D().dir.dot(ikparam.GetTra nslationDirection5D().dir);	dReal fcos = GetTranslationDirection5D().dir.dot(ikparam.GetTra nslationDirection5D().dir);
	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);
	return (GetTranslationDirection5D().pos-ikparam.GetTranslationD irection5D().pos).lengthsqr3() + anglemultfacosfacos;	return (GetTranslationDirection5D().pos-ikparam.GetTranslationD irection5D().pos).lengthsqr3() + anglemultfacosfacos;
	}	}
	case IkParameterization::Type_TranslationXY2D: {	case IkParameterization::Type_TranslationXY2D: {

	skipping to change at line 936	skipping to change at line 1042
	default:	default:
	BOOST_ASSERT(0);	BOOST_ASSERT(0);
	}	}
	return 1e30;	return 1e30;
	}	}

	protected:	protected:
	Transform _transform;	Transform _transform;
	Type _type;	Type _type;


	friend IkParameterization operator* (const Transform& t, const IkParame	friend IkParameterization operator* (const Transform &t, const IkParame
	terization& ikparam);	terization &ikparam);
	friend std::ostream& operator<<(std::ostream& O, const IkParameterizati	friend std::ostream& operator<<(std::ostream& O, const IkParameterizati
	on& ikparam);	on &ikparam);
	friend std::istream& operator>>(std::istream& I, IkParameterization& ik param);	friend std::istream& operator>>(std::istream& I, IkParameterization& ik param);
	};	};


	inline IkParameterization operator* (const Transform& t, const IkParameteri zation& ikparam)	inline IkParameterization operator* (const Transform &t, const IkParameteri zation &ikparam)
	{	{
	IkParameterization local;	IkParameterization local;
	switch(ikparam.GetType()) {	switch(ikparam.GetType()) {
	case IkParameterization::Type_Transform6D:	case IkParameterization::Type_Transform6D:
	local.SetTransform6D(t * ikparam.GetTransform6D());	local.SetTransform6D(t * ikparam.GetTransform6D());
	break;	break;
	case IkParameterization::Type_Rotation3D:	case IkParameterization::Type_Rotation3D:
	local.SetRotation3D(quatMultiply(quatInverse(t.rot),ikparam.GetRota tion3D()));	local.SetRotation3D(quatMultiply(quatInverse(t.rot),ikparam.GetRota tion3D()));
	break;	break;
	case IkParameterization::Type_Translation3D:	case IkParameterization::Type_Translation3D:

	skipping to change at line 986	skipping to change at line 1092
	}	}
	case IkParameterization::Type_TranslationLocalGlobal6D:	case IkParameterization::Type_TranslationLocalGlobal6D:
	local.SetTranslationLocalGlobal6D(ikparam.GetTranslationLocalGlobal 6D().first, t*ikparam.GetTranslationLocalGlobal6D().second);	local.SetTranslationLocalGlobal6D(ikparam.GetTranslationLocalGlobal 6D().first, t*ikparam.GetTranslationLocalGlobal6D().second);
	break;	break;
	default:	default:
	throw openrave_exception(str(boost::format("does not support parame terization %d")%ikparam.GetType()));	throw openrave_exception(str(boost::format("does not support parame terization %d")%ikparam.GetType()));
	}	}
	return local;	return local;
	}	}


	inline std::ostream& operator<<(std::ostream& O, const IkParameterization& ikparam)	inline std::ostream& operator<<(std::ostream& O, const IkParameterization & ikparam)
	{	{
	O << ikparam._type << " ";	O << ikparam._type << " ";
	switch(ikparam._type) {	switch(ikparam._type) {
	case IkParameterization::Type_Transform6D:	case IkParameterization::Type_Transform6D:
	O << ikparam.GetTransform6D();	O << ikparam.GetTransform6D();
	break;	break;
	case IkParameterization::Type_Rotation3D:	case IkParameterization::Type_Rotation3D:
	O << ikparam.GetRotation3D();	O << ikparam.GetRotation3D();
	break;	break;
	case IkParameterization::Type_Translation3D: {	case IkParameterization::Type_Translation3D: {

	skipping to change at line 1053	skipping to change at line 1159
	ikparam._type = static_cast<IkParameterization::Type>(type);	ikparam._type = static_cast<IkParameterization::Type>(type);
	switch(ikparam._type) {	switch(ikparam._type) {
	case IkParameterization::Type_Transform6D: { Transform t; I >> t; ikpar am.SetTransform6D(t); break; }	case IkParameterization::Type_Transform6D: { Transform t; I >> t; ikpar am.SetTransform6D(t); break; }
	case IkParameterization::Type_Rotation3D: { Vector v; I >> v; ikparam.S etRotation3D(v); break; }	case IkParameterization::Type_Rotation3D: { Vector v; I >> v; ikparam.S etRotation3D(v); break; }
	case IkParameterization::Type_Translation3D: { Vector v; I >> v.x >> v. y >> v.z; ikparam.SetTranslation3D(v); break; }	case IkParameterization::Type_Translation3D: { Vector v; I >> v.x >> v. y >> v.z; ikparam.SetTranslation3D(v); break; }
	case IkParameterization::Type_Direction3D: { Vector v; I >> v.x >> v.y >> v.z; ikparam.SetDirection3D(v); break; }	case IkParameterization::Type_Direction3D: { Vector v; I >> v.x >> v.y >> v.z; ikparam.SetDirection3D(v); break; }
	case IkParameterization::Type_Ray4D: { RAY r; I >> r; ikparam.SetRay4D( r); break; }	case IkParameterization::Type_Ray4D: { RAY r; I >> r; ikparam.SetRay4D( r); break; }
	case IkParameterization::Type_Lookat3D: { Vector v; I >> v.x >> v.y >> v.z; ikparam.SetLookat3D(v); break; }	case IkParameterization::Type_Lookat3D: { Vector v; I >> v.x >> v.y >> v.z; ikparam.SetLookat3D(v); break; }
	case IkParameterization::Type_TranslationDirection5D: { RAY r; I >> r; ikparam.SetTranslationDirection5D(r); break; }	case IkParameterization::Type_TranslationDirection5D: { RAY r; I >> r; ikparam.SetTranslationDirection5D(r); break; }
	case IkParameterization::Type_TranslationXY2D: { Vector v; I >> v.y >> v.y; ikparam.SetTranslationXY2D(v); break; }	case IkParameterization::Type_TranslationXY2D: { Vector v; I >> v.y >> v.y; ikparam.SetTranslationXY2D(v); break; }

	case IkParameterization::Type_TranslationXYOrientation3D: { Vector v; I >> v.y >> v.y >> v.z; ikparam.SetTranslationXYOrientation3D(v); break; }	case IkParameterization::Type_TranslationXYOrientation3D: { Vector v; I >> v.y >> v.y >> v.z; ikparam.SetTranslationXYOrientation3D(v); break; }
	case IkParameterization::Type_TranslationLocalGlobal6D: { Vector localt rans, trans; I >> localtrans.x >> localtrans.y >> localtrans.z >> trans.x > > trans.y >> trans.z; ikparam.SetTranslationLocalGlobal6D(localtrans,trans) ; break; }	case IkParameterization::Type_TranslationLocalGlobal6D: { Vector localt rans, trans; I >> localtrans.x >> localtrans.y >> localtrans.z >> trans.x > > trans.y >> trans.z; ikparam.SetTranslationLocalGlobal6D(localtrans,trans) ; break; }
	default: throw openrave_exception(str(boost::format("does not support p arameterization %d")%ikparam.GetType()));	default: throw openrave_exception(str(boost::format("does not support p arameterization %d")%ikparam.GetType()));
	}	}
	return I;	return I;
	}	}

	}	}

	#include <openrave/plugininfo.h>	#include <openrave/plugininfo.h>
	#include <openrave/interface.h>	#include <openrave/interface.h>

	skipping to change at line 1118	skipping to change at line 1224
	/// safely casts from the base interface class to an openrave interface usi ng static_pointer_cast.	/// safely casts from the base interface class to an openrave interface usi ng static_pointer_cast.
	/// The reason why dynamic_pointer_cast cannot be used is because interface s might be created by different plugins, and the runtime type information w ill be different.	/// The reason why dynamic_pointer_cast cannot be used is because interface s might be created by different plugins, and the runtime type information w ill be different.
	template <typename T>	template <typename T>
	inline boost::shared_ptr<T> RaveInterfaceCast(InterfaceBasePtr pinterface)	inline boost::shared_ptr<T> RaveInterfaceCast(InterfaceBasePtr pinterface)
	{	{
	if( !!pinterface ) {	if( !!pinterface ) {
	if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {	if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {
	return boost::static_pointer_cast<T>(pinterface);	return boost::static_pointer_cast<T>(pinterface);
	}	}
	// encode special cases	// encode special cases

	if( pinterface->GetInterfaceType() == PT_Robot && T::GetInterfaceTy peStatic() == PT_KinBody ) {	if((pinterface->GetInterfaceType() == PT_Robot)&&(T::GetInterfaceTy peStatic() == PT_KinBody)) {
	return boost::static_pointer_cast<T>(pinterface);	return boost::static_pointer_cast<T>(pinterface);
	}	}
	}	}
	return boost::shared_ptr<T>();	return boost::shared_ptr<T>();
	}	}

	/// safely casts from the base interface class to an openrave interface usi ng static_pointer_cast.	/// safely casts from the base interface class to an openrave interface usi ng static_pointer_cast.
	/// The reason why dynamic_pointer_cast cannot be used is because interface s might be created by different plugins, and the runtime type information w ill be different.	/// The reason why dynamic_pointer_cast cannot be used is because interface s might be created by different plugins, and the runtime type information w ill be different.
	template <typename T>	template <typename T>
	inline boost::shared_ptr<T const> RaveInterfaceConstCast(InterfaceBaseConst Ptr pinterface)	inline boost::shared_ptr<T const> RaveInterfaceConstCast(InterfaceBaseConst Ptr pinterface)
	{	{
	if( !!pinterface ) {	if( !!pinterface ) {
	if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {	if( pinterface->GetInterfaceType() == T::GetInterfaceTypeStatic() ) {
	return boost::static_pointer_cast<T const>(pinterface);	return boost::static_pointer_cast<T const>(pinterface);
	}	}
	// encode special cases	// encode special cases

	if( pinterface->GetInterfaceType() == PT_Robot && T::GetInterfaceTy peStatic() == PT_KinBody ) {	if((pinterface->GetInterfaceType() == PT_Robot)&&(T::GetInterfaceTy peStatic() == PT_KinBody)) {
	return boost::static_pointer_cast<T const>(pinterface);	return boost::static_pointer_cast<T const>(pinterface);
	}	}
	}	}
	return boost::shared_ptr<T>();	return boost::shared_ptr<T>();
	}	}

	/// \brief returns a lower case string of the interface type	/// \brief returns a lower case string of the interface type
	OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNam esMap();	OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNam esMap();
	OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type);	OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type);


	skipping to change at line 1224	skipping to change at line 1330
	OPENRAVE_API ControllerBasePtr RaveCreateController(EnvironmentBasePtr penv , const std::string& name);	OPENRAVE_API ControllerBasePtr RaveCreateController(EnvironmentBasePtr penv , const std::string& name);
	OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string& name);	OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string& name);
	OPENRAVE_API ModuleBasePtr RaveCreateProblem(EnvironmentBasePtr penv, const std::string& name);	OPENRAVE_API ModuleBasePtr RaveCreateProblem(EnvironmentBasePtr penv, const std::string& name);
	OPENRAVE_API ModuleBasePtr RaveCreateProblemInstance(EnvironmentBasePtr pen v, const std::string& name);	OPENRAVE_API ModuleBasePtr RaveCreateProblemInstance(EnvironmentBasePtr pen v, const std::string& name);
	OPENRAVE_API IkSolverBasePtr RaveCreateIkSolver(EnvironmentBasePtr penv, co nst std::string& name);	OPENRAVE_API IkSolverBasePtr RaveCreateIkSolver(EnvironmentBasePtr penv, co nst std::string& name);
	OPENRAVE_API PhysicsEngineBasePtr RaveCreatePhysicsEngine(EnvironmentBasePt r penv, const std::string& name);	OPENRAVE_API PhysicsEngineBasePtr RaveCreatePhysicsEngine(EnvironmentBasePt r penv, const std::string& name);
	OPENRAVE_API SensorBasePtr RaveCreateSensor(EnvironmentBasePtr penv, const std::string& name);	OPENRAVE_API SensorBasePtr RaveCreateSensor(EnvironmentBasePtr penv, const std::string& name);
	OPENRAVE_API CollisionCheckerBasePtr RaveCreateCollisionChecker(Environment BasePtr penv, const std::string& name);	OPENRAVE_API CollisionCheckerBasePtr RaveCreateCollisionChecker(Environment BasePtr penv, const std::string& name);
	OPENRAVE_API ViewerBasePtr RaveCreateViewer(EnvironmentBasePtr penv, const std::string& name);	OPENRAVE_API ViewerBasePtr RaveCreateViewer(EnvironmentBasePtr penv, const std::string& name);
	OPENRAVE_API SpaceSamplerBasePtr RaveCreateSpaceSampler(EnvironmentBasePtr penv, const std::string& name);	OPENRAVE_API SpaceSamplerBasePtr RaveCreateSpaceSampler(EnvironmentBasePtr penv, const std::string& name);

	OPENRAVE_API KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const s td::string& name="");	OPENRAVE_API KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const st d::string& name="");
	/// \brief Return an empty trajectory instance initialized to nDOF degrees of freedom. Will be deprecated soon	/// \brief Return an empty trajectory instance initialized to nDOF degrees of freedom. Will be deprecated soon
	OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , int nDOF);	OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , int nDOF);
	/// \brief Return an empty trajectory instance.	/// \brief Return an empty trajectory instance.
	OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , const std::string& name="");	OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv , const std::string& name="");

	/** \brief Registers a function to create an interface, this allows the int erface to be created by other modules.	/** \brief Registers a function to create an interface, this allows the int erface to be created by other modules.

	\param type interface type	\param type interface type
	\param name interface name	\param name interface name
	\param interfacehash the hash of the interface being created (use the g lobal defines OPENRAVE_X_HASH)	\param interfacehash the hash of the interface being created (use the g lobal defines OPENRAVE_X_HASH)

	skipping to change at line 1249	skipping to change at line 1355
	*/	*/
	OPENRAVE_API boost::shared_ptr<void> RaveRegisterInterface(InterfaceType ty pe, const std::string& name, const char* interfacehash, const char* envhash , const boost::function<InterfaceBasePtr(EnvironmentBasePtr, std::istream&) >& createfn);	OPENRAVE_API boost::shared_ptr<void> RaveRegisterInterface(InterfaceType ty pe, const std::string& name, const char* interfacehash, const char* envhash , const boost::function<InterfaceBasePtr(EnvironmentBasePtr, std::istream&) >& createfn);

	/** \brief Registers a custom xml reader for a particular interface.	/** \brief Registers a custom xml reader for a particular interface.

	Once registered, anytime an interface is created through XML and	Once registered, anytime an interface is created through XML and
	the xmltag is seen, the function CreateXMLReaderFn will be called to ge t a reader for that tag	the xmltag is seen, the function CreateXMLReaderFn will be called to ge t a reader for that tag
	\param xmltag the tag specified in xmltag is seen in the interface, the the custom reader will be created.	\param xmltag the tag specified in xmltag is seen in the interface, the the custom reader will be created.
	\param fn CreateXMLReaderFn(pinterface,atts) - passed in the pointer to the interface where the tag was seen along with the list of attributes	\param fn CreateXMLReaderFn(pinterface,atts) - passed in the pointer to the interface where the tag was seen along with the list of attributes
	\return a pointer holding the registration, releasing the pointer will unregister the XML reader	\return a pointer holding the registration, releasing the pointer will unregister the XML reader

	*/	*/
	OPENRAVE_API boost::shared_ptr<void> RaveRegisterXMLReader(InterfaceType ty pe, const std::string& xmltag, const CreateXMLReaderFn& fn);	OPENRAVE_API boost::shared_ptr<void> RaveRegisterXMLReader(InterfaceType ty pe, const std::string& xmltag, const CreateXMLReaderFn& fn);

	/// \brief return the environment's unique id, returns 0 if environment cou ld not be found or not registered	/// \brief return the environment's unique id, returns 0 if environment cou ld not be found or not registered
	OPENRAVE_API int RaveGetEnvironmentId(EnvironmentBasePtr penv);	OPENRAVE_API int RaveGetEnvironmentId(EnvironmentBasePtr penv);

	/// \brief get the environment from its unique id	/// \brief get the environment from its unique id
	/// \param id the unique environment id returned by \ref RaveGetEnvironment Id	/// \param id the unique environment id returned by \ref RaveGetEnvironment Id
	OPENRAVE_API EnvironmentBasePtr RaveGetEnvironment(int id);	OPENRAVE_API EnvironmentBasePtr RaveGetEnvironment(int id);

	/// \brief Return all the created OpenRAVE environments.	/// \brief Return all the created OpenRAVE environments.

	skipping to change at line 1292	skipping to change at line 1398
	if( !pdirs ) {	if( !pdirs ) {
	return false;	return false;
	}	}
	// search for all directories separated by ':'	// search for all directories separated by ':'
	std::string tmp = pdirs;	std::string tmp = pdirs;
	std::string::size_type pos = 0, newpos=0;	std::string::size_type pos = 0, newpos=0;
	while( pos < tmp.size() ) {	while( pos < tmp.size() ) {
	#ifdef _WIN32	#ifdef _WIN32
	newpos = tmp.find(';', pos);	newpos = tmp.find(';', pos);
	#else	#else

	newpos = tmp.find(':', pos);	newpos = tmp.find(':', pos);
	#endif	#endif
	std::string::size_type n = newpos == std::string::npos ? tmp.size() -pos : (newpos-pos);	std::string::size_type n = newpos == std::string::npos ? tmp.size() -pos : (newpos-pos);
	vdirs.push_back(tmp.substr(pos, n));	vdirs.push_back(tmp.substr(pos, n));
	if( newpos == std::string::npos ) {	if( newpos == std::string::npos ) {
	break;	break;
	}	}
	pos = newpos+1;	pos = newpos+1;
	}	}
	return true;	return true;
	}	}

End of changes. 65 change blocks.
	226 lines changed or deleted	304 lines changed or added

	planningutils.h	planningutils.h

	skipping to change at line 20	skipping to change at line 20
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	// GNU Lesser General Public License for more details.	// GNU Lesser General Public License for more details.
	//	//
	// You should have received a copy of the GNU Lesser General Public License	// You should have received a copy of the GNU Lesser General Public License
	// along with this program. If not, see <http://www.gnu.org/licenses/>.	// along with this program. If not, see <http://www.gnu.org/licenses/>.

	/** \file planningutils.h	/** \file planningutils.h
	\brief Planning related utilities likes samplers, distance metrics, etc .	\brief Planning related utilities likes samplers, distance metrics, etc .

	*/	*/
	#ifndef OPENRAVE_PLANNINGUTIL_H	#ifndef OPENRAVE_PLANNINGUTIL_H
	#define OPENRAVE_PLANNINGUTIL_H	#define OPENRAVE_PLANNINGUTIL_H

	#include <openrave/openrave.h>	#include <openrave/openrave.h>

	namespace OpenRAVE {	namespace OpenRAVE {

	namespace planningutils {	namespace planningutils {


	/// \brief Jitters the active joint angles of the robot until it escape	/// \brief Jitters the active joint angles of the robot until it escapes co
	s collision.	llision.
	///	///
	/// Return 0 if jitter failed and robot is in collision, -1 if robot or	/// Return 0 if jitter failed and robot is in collision, -1 if robot origin
	iginally not in collision, 1 if jitter succeeded.	ally not in collision, 1 if jitter succeeded.
	OPENRAVE_API int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations=	OPENRAVE_API int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations=5000
	5000,dReal fRand=0.03f,const PlannerBase::PlannerParameters::NeighStateFn&	,dReal fRand=0.03f,const PlannerBase::PlannerParameters::NeighStateFn& neig
	neighstatefn = PlannerBase::PlannerParameters::NeighStateFn());	hstatefn = PlannerBase::PlannerParameters::NeighStateFn());


	/// \brief Jitters the transform of a body until it escapes collision.	/// \brief Jitters the transform of a body until it escapes collision.
	OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int	OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMax
	nMaxIterations=1000);	Iterations=1000);


	/** \brief validates a trajectory with respect to the planning constrai nts.	/** \brief validates a trajectory with respect to the planning constraints.


	checks internal data structures and verifies that all trajectory vi	checks internal data structures and verifies that all trajectory via po
	a points do not violate joint position, velocity, and acceleration limits.	ints do not violate joint position, velocity, and acceleration limits.
	\param trajectory trajectory of points to be checked	\param trajectory trajectory of points to be checked
	\param parameters the planner parameters passed to the planner that	\param parameters the planner parameters passed to the planner that ret
	returned the trajectory	urned the trajectory
	\param samplingstep If == 0, then will only test the supports point	\param samplingstep If == 0, then will only test the supports points in
	s in trajectory->GetPoints(). If > 0, then will sample the trajectory at th	trajectory->GetPoints(). If > 0, then will sample the trajectory at this t
	is time interval.	ime interval.
	\throw openrave_exception If the trajectory is invalid, will throw	\throw openrave_exception If the trajectory is invalid, will throw ORE_
	ORE_InconsistentConstraints.	InconsistentConstraints.
	*/	*/
	OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstP	OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p
	tr parameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0);	arameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0);


	/// \brief Line collision	/// \brief Line collision
	class OPENRAVE_API LineCollisionConstraint	class OPENRAVE_API LineCollisionConstraint
	{	{
	public:	public:
	LineCollisionConstraint();	LineCollisionConstraint();
	bool Check(PlannerBase::PlannerParametersWeakPtr _params, RobotBase	bool Check(PlannerBase::PlannerParametersWeakPtr _params, RobotBasePtr
	Ptr robot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, In	robot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, Interv
	tervalType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurati	alType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations)
	ons);	;


	protected:	protected:
	std::vector<dReal> _vtempconfig, dQ;	std::vector<dReal> _vtempconfig, dQ;
	CollisionReportPtr _report;	CollisionReportPtr _report;
	};	};


	/// \brief simple distance metric based on joint weights	/// \brief simple distance metric based on joint weights
	class OPENRAVE_API SimpleDistanceMetric	class OPENRAVE_API SimpleDistanceMetric
	{	{
	public:	public:
	SimpleDistanceMetric(RobotBasePtr robot);	SimpleDistanceMetric(RobotBasePtr robot);
	dReal Eval(const std::vector<dReal>& c0, const std::vector<dReal>&	dReal Eval(const std::vector<dReal>& c0, const std::vector<dReal>& c1);
	c1);	protected:
	protected:	RobotBasePtr _robot;
	RobotBasePtr _robot;	std::vector<dReal> weights2;
	std::vector<dReal> weights2;	};
	};


	/// \brief samples the neighborhood of a configuration using the config	/// \brief samples the neighborhood of a configuration using the configurat
	uration space distance metric and sampler.	ion space distance metric and sampler.
	class OPENRAVE_API SimpleNeighborhoodSampler	class OPENRAVE_API SimpleNeighborhoodSampler
	{	{
	public:	public:
	SimpleNeighborhoodSampler(SpaceSamplerBasePtr psampler, const boost	SimpleNeighborhoodSampler(SpaceSamplerBasePtr psampler, const boost::fu
	::function<dReal(const std::vector<dReal>&, const std::vector<dReal>&)>& di	nction<dReal(const std::vector<dReal>&, const std::vector<dReal>&)>&distmet
	stmetricfn);	ricfn);


	bool Sample(std::vector<dReal>& vNewSample, const std::vector<dReal	bool Sample(std::vector<dReal>& vNewSample, const std::vector<dReal>& v
	>& vCurSample, dReal fRadius);	CurSample, dReal fRadius);
	bool Sample(std::vector<dReal>& samples);	bool Sample(std::vector<dReal>& samples);
	protected:	protected:
	SpaceSamplerBasePtr _psampler;	SpaceSamplerBasePtr _psampler;
	boost::function<dReal(const std::vector<dReal>&, const std::vector<	boost::function<dReal(const std::vector<dReal>&, const std::vector<dRea
	dReal>&)> _distmetricfn;	l>&)> _distmetricfn;
	};	};


	/// \brief Samples numsamples of solutions and each solution to vsoluti	/// \brief Samples numsamples of solutions and each solution to vsolutions
	ons	class OPENRAVE_API ManipulatorIKGoalSampler
	class OPENRAVE_API ManipulatorIKGoalSampler	{
	{	public:
	public:	ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const s
	ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, con	td::list<IkParameterization>&listparameterizations, int nummaxsamples=20, i
	st std::list<IkParameterization>& listparameterizations, int nummaxsamples=	nt nummaxtries=10, dReal fsampleprob=0.05f);
	20, int nummaxtries=10, dReal fsampleprob=0.05f);	//void SetCheckPathConstraintsFn(const PlannerBase::PlannerParameters::
	//void SetCheckPathConstraintsFn(const PlannerBase::PlannerParamete	CheckPathConstraintFn& checkfn)
	rs::CheckPathConstraintFn& checkfn)	bool Sample(std::vector<dReal>& vgoal);
	bool Sample(std::vector<dReal>& vgoal);	int GetIkParameterizationIndex(int index);
	int GetIkParameterizationIndex(int index);


	protected:	protected:
	struct SampleInfo	struct SampleInfo
	{	{
	IkParameterization _ikparam;	IkParameterization _ikparam;
	int _numleft;	int _numleft;
	SpaceSamplerBasePtr _psampler;	SpaceSamplerBasePtr _psampler;
	int _orgindex;	int _orgindex;
	};
	RobotBasePtr _probot;
	RobotBase::ManipulatorConstPtr _pmanip;
	int _nummaxsamples, _nummaxtries;
	std::list<SampleInfo> _listsamples;
	SpaceSamplerBasePtr _pindexsampler;
	dReal _fsampleprob;
	CollisionReportPtr _report;
	std::vector< std::vector<dReal> > _viksolutions;
	std::list<int> _listreturnedsamples;
	std::vector<dReal> _vfreestart;
	};	};

		RobotBasePtr _probot;
		RobotBase::ManipulatorConstPtr _pmanip;
		int _nummaxsamples, _nummaxtries;
		std::list<SampleInfo> _listsamples;
		SpaceSamplerBasePtr _pindexsampler;
		dReal _fsampleprob;
		CollisionReportPtr _report;
		std::vector< std::vector<dReal> > _viksolutions;
		std::list<int> _listreturnedsamples;
		std::vector<dReal> _vfreestart;
		};

	} // planningutils	} // planningutils
	} // OpenRAVE	} // OpenRAVE

	#endif	#endif

End of changes. 13 change blocks.
	96 lines changed or deleted	94 lines changed or added

	plugininfo.h	plugininfo.h

	skipping to change at line 29	skipping to change at line 29
	*/	*/
	#ifndef OPENRAVE_PLUGIN_INFO	#ifndef OPENRAVE_PLUGIN_INFO
	#define OPENRAVE_PLUGIN_INFO	#define OPENRAVE_PLUGIN_INFO

	namespace OpenRAVE {	namespace OpenRAVE {

	/** \brief Holds all the %OpenRAVE-specific information provided by a plugi n.	/** \brief Holds all the %OpenRAVE-specific information provided by a plugi n.

	\ingroup plugin_exports	\ingroup plugin_exports
	PLUGININFO has a hash computed for it to validate its size and type bef ore having a plugin fill it.	PLUGININFO has a hash computed for it to validate its size and type bef ore having a plugin fill it.

	*/	*/
	class OPENRAVE_API PLUGININFO	class OPENRAVE_API PLUGININFO
	{	{
	public:	public:

	PLUGININFO() : version(0) {}	PLUGININFO() : version(0) {
	std::map<InterfaceType, std::vector<std::string> > interfacenames; ///<	}
	offered interfaces	std::map<InterfaceType, std::vector<std::string> > interfacenames;
	int version; ///< OPENRAVE_VERSION	///< offered interfaces
		int version; ///< OPENRAVE_VERSION
	};	};

	}	}

	#endif	#endif

End of changes. 2 change blocks.
	5 lines changed or deleted	6 lines changed or added

	sensor.h	sensor.h

	skipping to change at line 19	skipping to change at line 19
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	// GNU Lesser General Public License for more details.	// GNU Lesser General Public License for more details.
	//	//
	// You should have received a copy of the GNU Lesser General Public License	// You should have received a copy of the GNU Lesser General Public License
	// along with this program. If not, see <http://www.gnu.org/licenses/>.	// along with this program. If not, see <http://www.gnu.org/licenses/>.
	/** \file sensor.h	/** \file sensor.h
	\brief Sensor and sensing related defintions.	\brief Sensor and sensing related defintions.

	*/	*/
	#ifndef OPENRAVE_SENSOR_H	#ifndef OPENRAVE_SENSOR_H
	#define OPENRAVE_SENSOR_H	#define OPENRAVE_SENSOR_H

	namespace OpenRAVE {	namespace OpenRAVE {

	/** \brief <b>[interface]</b> A sensor measures physical properties from th e environment. See \ref arch_sensor.	/** \brief <b>[interface]</b> A sensor measures physical properties from th e environment. See \ref arch_sensor.

	\ingroup interfaces	\ingroup interfaces
	*/	*/
	class OPENRAVE_API SensorBase : public InterfaceBase	class OPENRAVE_API SensorBase : public InterfaceBase
	{	{
	public:	public:
	enum SensorType	enum SensorType
	{	{
	ST_Invalid=0,	ST_Invalid=0,
	ST_Laser=1,	ST_Laser=1,
	ST_Camera=2,	ST_Camera=2,
	ST_JointEncoder=3,	ST_JointEncoder=3,
	ST_Force6D=4,	ST_Force6D=4,

	skipping to change at line 50	skipping to change at line 50
	ST_Tactile=7,	ST_Tactile=7,
	ST_Actuator=8,	ST_Actuator=8,
	ST_NumberofSensorTypes=8	ST_NumberofSensorTypes=8
	};	};

	typedef geometry::RaveCameraIntrinsics<dReal> CameraIntrinsics;	typedef geometry::RaveCameraIntrinsics<dReal> CameraIntrinsics;

	/// used to pass sensor data around	/// used to pass sensor data around
	class OPENRAVE_API SensorData	class OPENRAVE_API SensorData
	{	{

	public:	public:
	virtual ~SensorData() {}	virtual ~SensorData() {
		}
	virtual SensorType GetType() = 0;	virtual SensorType GetType() = 0;

	/// Serialize the sensor data to stream in XML format	/// Serialize the sensor data to stream in XML format
	virtual bool serialize(std::ostream& O) const;	virtual bool serialize(std::ostream& O) const;


	uint64_t __stamp; ///< time stamp of the sensor data in microsecond	uint64_t __stamp; ///< time stamp of the sensor data in mic
	s. If 0, then the data is uninitialized! (floating-point precision is bad h	roseconds. If 0, then the data is uninitialized! (floating-point precision
	ere). This can be either simulation or real time depending on the sensor.	is bad here). This can be either simulation or real time depending on the s
	Transform __trans; ///< the coordinate system the sensor was wh	ensor.
	en the measurement was taken, this is taken directly from SensorBase::GetTr	Transform __trans; ///< the coordinate system the senso
	ansform	r was when the measurement was taken, this is taken directly from SensorBas
		e::GetTransform
	};	};
	typedef boost::shared_ptr<SensorBase::SensorData> SensorDataPtr;	typedef boost::shared_ptr<SensorBase::SensorData> SensorDataPtr;
	typedef boost::shared_ptr<SensorBase::SensorData const> SensorDataConst Ptr;	typedef boost::shared_ptr<SensorBase::SensorData const> SensorDataConst Ptr;

	class OPENRAVE_API LaserSensorData : public SensorData	class OPENRAVE_API LaserSensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Laser; }	virtual SensorType GetType() {
		return ST_Laser;
		}


	std::vector<RaveVector<dReal> > positions; ///< world coordinates o	std::vector<RaveVector<dReal> > positions; ///< world coord
	f the origins of each of the laser points.	inates of the origins of each of the laser points.
	///< if positions is empty, assume t	///< if positions is empty, assume the origin is t.trans for all po
	he origin is t.trans for all points	ints
	std::vector<RaveVector<dReal> > ranges; ///< Range and direction re	std::vector<RaveVector<dReal> > ranges; ///< Range and dire
	adings in the form of direction*distance. The direction is in world coordin	ction readings in the form of direction*distance. The direction is in world
	ates. The values should be returned in the order laser detected them in.	coordinates. The values should be returned in the order laser detected the
	std::vector<dReal> intensity; ///< Intensity readings.	m in.
		std::vector<dReal> intensity; ///< Intensity readings.

	virtual bool serialize(std::ostream& O) const;	virtual bool serialize(std::ostream& O) const;
	};	};
	class OPENRAVE_API CameraSensorData : public SensorData	class OPENRAVE_API CameraSensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Camera; }	virtual SensorType GetType() {
	std::vector<uint8_t> vimagedata; ///< rgb image data, if camera onl	return ST_Camera;
	y outputs in grayscale, fill each channel with the same value	}
		std::vector<uint8_t> vimagedata; ///< rgb image data, if ca
		mera only outputs in grayscale, fill each channel with the same value
	virtual bool serialize(std::ostream& O) const;	virtual bool serialize(std::ostream& O) const;
	};	};

	/// \brief Stores joint angles and EE position.	/// \brief Stores joint angles and EE position.
	class OPENRAVE_API JointEncoderSensorData : public SensorData	class OPENRAVE_API JointEncoderSensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_JointEncoder; }	virtual SensorType GetType() {
	std::vector<dReal> encoderValues; ///< measured joint angles in rad	return ST_JointEncoder;
	ians	}
	std::vector<dReal> encoderVelocity; ///< measured joint velocity in	std::vector<dReal> encoderValues; ///< measured joint angle
	radians	s in radians
		std::vector<dReal> encoderVelocity; ///< measured joint vel
		ocity in radians
	};	};

	/// \brief Stores force data	/// \brief Stores force data
	class OPENRAVE_API Force6DSensorData : public SensorData	class OPENRAVE_API Force6DSensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Force6D; }	virtual SensorType GetType() {
	Vector force; ///< Force in X Y Z, in newtons	return ST_Force6D;
	Vector torque; ///< Torque in X Y Z, in newtonmeters	}
		Vector force; ///< Force in X Y Z, in newtons
		Vector torque; ///< Torque in X Y Z, in newtonmeters
	};	};

	/// \brief Stores IMU data	/// \brief Stores IMU data
	class OPENRAVE_API IMUSensorData : public SensorData	class OPENRAVE_API IMUSensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_IMU; }	virtual SensorType GetType() {
	Vector rotation; ///< quaternion	return ST_IMU;
		}
		Vector rotation; ///< quaternion
	Vector angular_velocity;	Vector angular_velocity;
	Vector linear_acceleration;	Vector linear_acceleration;

	boost::array<dReal,9> rotation_covariance; ///< Row major about x,	boost::array<dReal,9> rotation_covariance; ///< Row major a
	y, z axes	bout x, y, z axes
	boost::array<dReal,9> angular_velocity_covariance; ///< Row major a	boost::array<dReal,9> angular_velocity_covariance; ///< Row
	bout x, y, z axes	major about x, y, z axes
	boost::array<dReal,9> linear_acceleration_covariance; ///< Row majo	boost::array<dReal,9> linear_acceleration_covariance; ///<
	r x, y z axes	Row major x, y z axes
	};	};

	/// \brief odometry data storing full 6D pose and velocity	/// \brief odometry data storing full 6D pose and velocity
	class OPENRAVE_API OdometrySensorData : public SensorData	class OPENRAVE_API OdometrySensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Odometry; }	virtual SensorType GetType() {
	Transform pose; ///< measured pose	return ST_Odometry;
	Vector linear_velocity, angular_velocity; ///< measured velocity	}
	boost::array<dReal,36> pose_covariance; ///< Row major of 6x6 matri	Transform pose; ///< measured pose
	x about linear x, y, z axes	Vector linear_velocity, angular_velocity; ///< measured vel
	boost::array<dReal,36> velocity_covariance; ///< Row major of 6x6 m	ocity
	atrix about rotational x, y, z axes	boost::array<dReal,36> pose_covariance; ///< Row major of 6
		x6 matrix about linear x, y, z axes
		boost::array<dReal,36> velocity_covariance; ///< Row major
		of 6x6 matrix about rotational x, y, z axes
	};	};

	/// \brief tactle data	/// \brief tactle data
	class OPENRAVE_API TactileSensorData : public SensorData	class OPENRAVE_API TactileSensorData : public SensorData
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Tactile; }	virtual SensorType GetType() {
	std::vector<Vector> forces; /// xyz force of each individual elemen	return ST_Tactile;
	t	}
	boost::array<dReal,9> force_covariance; ///< row major 3x3 matrix o	std::vector<Vector> forces; /// xyz force of each individua
	f the uncertainty on the xyz force measurements	l element
		boost::array<dReal,9> force_covariance; ///< row major 3x3
		matrix of the uncertainty on the xyz force measurements
	};	};

	/// \brief An actuator for modeling motors and other mechanisms that pr oduce torque/force. The actuator has only one degree of freedom.	/// \brief An actuator for modeling motors and other mechanisms that pr oduce torque/force. The actuator has only one degree of freedom.
	class OPENRAVE_API ActuatorSensorData : public SensorData	class OPENRAVE_API ActuatorSensorData : public SensorData
	{	{

	public:	public:
	/// \brief the state of the actuator	/// \brief the state of the actuator
	enum ActuatorState {	enum ActuatorState {

	AS_Undefined=0, ///< returned when no state is defined	AS_Undefined=0, ///< returned when no state is defined
	AS_Idle=1, ///< this actuator is idle	AS_Idle=1, ///< this actuator is idle
	AS_Moving=2, ///< this actuator is in motion from previous comm	AS_Moving=2, ///< this actuator is in motion from previ
	ands	ous commands
	AS_Stalled=3, ///< the actuator is stalled, needs to be unstall	AS_Stalled=3, ///< the actuator is stalled, needs to be
	ed by sending a ready signal	unstalled by sending a ready signal
	AS_Braked=4, ///< the actuator is braked	AS_Braked=4, ///< the actuator is braked
	};	};


	ActuatorSensorData() : state(AS_Undefined), measuredcurrent(0), mea	ActuatorSensorData() : state(AS_Undefined), measuredcurrent(0), mea
	suredtemperature(0), appliedcurrent(0) {}	suredtemperature(0), appliedcurrent(0) {
	virtual SensorType GetType() { return ST_Actuator; }	}
		virtual SensorType GetType() {
		return ST_Actuator;
		}

	ActuatorState state;	ActuatorState state;

	dReal measuredcurrent; ///< measured current from the actuator	dReal measuredcurrent; ///< measured current from the actua
	dReal measuredtemperature; ///< measured temperature from the actua	tor
	tor	dReal measuredtemperature; ///< measured temperature from t
	dReal appliedcurrent; ///< current sent to the actuator	he actuator
		dReal appliedcurrent; ///< current sent to the actuator
	};	};

	/// permanent properties of the sensors	/// permanent properties of the sensors
	class OPENRAVE_API SensorGeometry	class OPENRAVE_API SensorGeometry
	{	{

	public:	public:
	virtual ~SensorGeometry() {}	virtual ~SensorGeometry() {
		}
	virtual SensorType GetType() = 0;	virtual SensorType GetType() = 0;
	};	};
	typedef boost::shared_ptr<SensorBase::SensorGeometry> SensorGeometryPtr ;	typedef boost::shared_ptr<SensorBase::SensorGeometry> SensorGeometryPtr ;
	typedef boost::shared_ptr<SensorBase::SensorGeometry const> SensorGeome tryConstPtr;	typedef boost::shared_ptr<SensorBase::SensorGeometry const> SensorGeome tryConstPtr;

	class OPENRAVE_API LaserGeomData : public SensorGeometry	class OPENRAVE_API LaserGeomData : public SensorGeometry
	{	{

	public:	public:
	LaserGeomData() : min_range(0), max_range(0), time_increment(0), time_s	LaserGeomData() : min_range(0), max_range(0), time_increment(0), ti
	can(0) { min_angle[0] = min_angle[1] = max_angle[0] = max_angle[1] = resolu	me_scan(0) {
	tion[0] = resolution[1] = 0; }	min_angle[0] = min_angle[1] = max_angle[0] = max_angle[1] = res
	virtual SensorType GetType() { return ST_Laser; }	olution[0] = resolution[1] = 0;
	boost::array<dReal,2> min_angle; ///< Start for the laser scan [rad	}
	].	virtual SensorType GetType() {
	boost::array<dReal,2> max_angle; ///< End angles for the laser scan	return ST_Laser;
	[rad].	}
	boost::array<dReal,2> resolution; ///< Angular resolutions for each	boost::array<dReal,2> min_angle; ///< Start for the laser s
	axis of rotation [rad].	can [rad].
	dReal min_range, max_range; ///< Maximum range [m].	boost::array<dReal,2> max_angle; ///< End angles for the la
	dReal time_increment; ///< time between individual measurements [se	ser scan [rad].
	conds]	boost::array<dReal,2> resolution; ///< Angular resolutions
	dReal time_scan; ///< time between scans [seconds]	for each axis of rotation [rad].
		dReal min_range, max_range; ///< Maximum range [m].
		dReal time_increment; ///< time between individual measurem
		ents [seconds]
		dReal time_scan; ///< time between scans [seconds]
	};	};
	class OPENRAVE_API CameraGeomData : public SensorGeometry	class OPENRAVE_API CameraGeomData : public SensorGeometry
	{	{

	public:	public:
	CameraGeomData() : width(0), height(0) {}	CameraGeomData() : width(0), height(0) {
	virtual SensorType GetType() { return ST_Camera; }	}
	CameraIntrinsics KK; ///< intrinsic matrix	virtual SensorType GetType() {
	int width, height; ///< width and height of image	return ST_Camera;
		}
		CameraIntrinsics KK; ///< intrinsic matrix
		int width, height; ///< width and height of image
	};	};
	class OPENRAVE_API JointEncoderGeomData : public SensorGeometry	class OPENRAVE_API JointEncoderGeomData : public SensorGeometry
	{	{

	public:	public:
	JointEncoderGeomData() : resolution(0) {}	JointEncoderGeomData() : resolution(0) {
	virtual SensorType GetType() { return ST_JointEncoder; }	}
	std::vector<dReal> resolution; ///< the delta value of one encoder	virtual SensorType GetType() {
	tick	return ST_JointEncoder;
		}
		std::vector<dReal> resolution; ///< the delta value of one
		encoder tick
	};	};
	class OPENRAVE_API Force6DGeomData : public SensorGeometry	class OPENRAVE_API Force6DGeomData : public SensorGeometry
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Force6D; }	virtual SensorType GetType() {
		return ST_Force6D;
		}
	};	};
	class OPENRAVE_API IMUGeomData : public SensorGeometry	class OPENRAVE_API IMUGeomData : public SensorGeometry
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_IMU; }	virtual SensorType GetType() {
	dReal time_measurement; ///< time between measurements	return ST_IMU;
		}
		dReal time_measurement; ///< time between measurements
	};	};
	class OPENRAVE_API OdometryGeomData : public SensorGeometry	class OPENRAVE_API OdometryGeomData : public SensorGeometry
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Odometry; }	virtual SensorType GetType() {
	std::string targetid; ///< id of the target whose odometry/pose mes	return ST_Odometry;
	sages are being published for	}
		std::string targetid; ///< id of the target whose odometry/
		pose messages are being published for
	};	};

	class OPENRAVE_API TactileGeomData : public SensorGeometry	class OPENRAVE_API TactileGeomData : public SensorGeometry
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Tactile; }	virtual SensorType GetType() {
		return ST_Tactile;
		}

	/// LuGre friction model?	/// LuGre friction model?
	struct Friction	struct Friction
	{	{

	dReal sigma_0; ///< the stiffness coefficient of the contacting	dReal sigma_0; ///< the stiffness coefficient of the co
	surfaces	ntacting surfaces
	dReal sigma_1; ///< the friction damping coefficient.	dReal sigma_1; ///< the friction damping coefficient.
	dReal mu_s; ///< static friction coefficient	dReal mu_s; ///< static friction coefficient
	dReal mu_d; ///< dynamic friction coefficient	dReal mu_d; ///< dynamic friction coefficient
	};	};

	std::vector<Vector> positions; ///< 3D positions of all the element	std::vector<Vector> positions; ///< 3D positions of all the
	s in the sensor frame	elements in the sensor frame
	dReal thickness; ///< the thickness of the tactile sensors (used fo	dReal thickness; ///< the thickness of the tactile sensors
	r determining contact and computing force)	(used for determining contact and computing force)
	///dReal normal_force_stiffness, normal_force_damping; ///< simple model for determining contact force from depressed distance... necessary?	///dReal normal_force_stiffness, normal_force_damping; ///< simple model for determining contact force from depressed distance... necessary?

	std::map<std::string, Friction> _mapfriction; ///< friction coeffic ients references by target objects	std::map<std::string, Friction> _mapfriction; ///< friction coefficients references by target objects
	};	};

	class OPENRAVE_API ActuatorGeomData : public SensorGeometry	class OPENRAVE_API ActuatorGeomData : public SensorGeometry
	{	{

	public:	public:
	virtual SensorType GetType() { return ST_Actuator; }	virtual SensorType GetType() {
	dReal maxtorque; ///< Maximum possible torque actuator can apply (o	return ST_Actuator;
	n output side). This includes the actuator's rotor, if one exists.	}
	dReal maxcurrent; ///< Maximum permissible current of the actuator.	dReal maxtorque; ///< Maximum possible torque actuator can
	If this current value is exceeded for a prolonged period of time, then an	apply (on output side). This includes the actuator's rotor, if one exists.
	error could occur (due to heat, etc).	dReal maxcurrent; ///< Maximum permissible current of the a
	dReal nominalcurrent; ///< Rated current of the actuator.	ctuator. If this current value is exceeded for a prolonged period of time,
	dReal maxvelocity; ///< Maximum permissible velocity of the system	then an error could occur (due to heat, etc).
	(on output side).	dReal nominalcurrent; ///< Rated current of the actuator.
	dReal maxacceleration; ///< Maximum permissible acceleration of the	dReal maxvelocity; ///< Maximum permissible velocity of the
	system (on output side).	system (on output side).
	dReal maxjerk; ///< Maximum permissible jerking of the system (on o	dReal maxacceleration; ///< Maximum permissible acceleratio
	utput side). The jerk results from a sudden change in acceleration.	n of the system (on output side).
	dReal staticfriction; ///< minimum torque that must be applied for	dReal maxjerk; ///< Maximum permissible jerking of the syst
	actuator to overcome static friction	em (on output side). The jerk results from a sudden change in acceleration.
	dReal viscousfriction; ///< friction depending on the velocity of t	dReal staticfriction; ///< minimum torque that must be appl
	he actuator	ied for actuator to overcome static friction
		dReal viscousfriction; ///< friction depending on the veloc
		ity of the actuator
	};	};


	SensorBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Sensor, penv) {}	SensorBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Sensor, penv) {
	virtual ~SensorBase() {}	}
		virtual ~SensorBase() {
		}

	/// return the static interface type this class points to (used for saf e casting)	/// return the static interface type this class points to (used for saf e casting)

	static inline InterfaceType GetInterfaceTypeStatic() { return PT_Sensor	static inline InterfaceType GetInterfaceTypeStatic() {
	; }	return PT_Sensor;
		}

	/// \brief A set of commands used for run-time sensor configuration.	/// \brief A set of commands used for run-time sensor configuration.
	enum ConfigureCommand	enum ConfigureCommand
	{	{

	CC_PowerOn=0x10, ///< turns the sensor on, starts gathering data an	CC_PowerOn=0x10, ///< turns the sensor on, starts gathering dat
	d using processor cycles. If the power is already on, servers as a reset. (	a and using processor cycles. If the power is already on, servers as a rese
	off by default)	t. (off by default)
	CC_PowerOff=0x11, ///< turns the sensor off, stops gathering data (	CC_PowerOff=0x11, ///< turns the sensor off, stops gathering da
	off by default).	ta (off by default).
	CC_PowerCheck=0x12, ///< returns whether power is on	CC_PowerCheck=0x12, ///< returns whether power is on
	CC_RenderDataOn=0x20, ///< turns on any rendering of the sensor dat	CC_RenderDataOn=0x20, ///< turns on any rendering of the sensor
	a (off by default)	data (off by default)
	CC_RenderDataOff=0x21, ///< turns off any rendering of the sensor d	CC_RenderDataOff=0x21, ///< turns off any rendering of the sens
	ata (off by default)	or data (off by default)
	CC_RenderDataCheck=0x23, ///< returns whether data rendering is on	CC_RenderDataCheck=0x23, ///< returns whether data rendering is
	CC_RenderGeometryOn=0x30, ///< turns on any rendering of the sensor	on
	geometry (on by default)	CC_RenderGeometryOn=0x30, ///< turns on any rendering of the se
	CC_RenderGeometryOff=0x31, ///< turns off any rendering of the sens	nsor geometry (on by default)
	or geometry (on by default)	CC_RenderGeometryOff=0x31, ///< turns off any rendering of the
	CC_RenderGeometryCheck=0x32, ///< returns whether geometry renderin	sensor geometry (on by default)
	g is on	CC_RenderGeometryCheck=0x32, ///< returns whether geometry rend
		ering is on
	};	};

	/// \brief Configures properties of the sensor like power.	/// \brief Configures properties of the sensor like power.
	///	///
	/// \param type \ref ConfigureCommand	/// \param type \ref ConfigureCommand
	/// \param blocking If set to true, makes sure the configuration ends b efore this function returns.(might cause problems if environment is locked) .	/// \param blocking If set to true, makes sure the configuration ends b efore this function returns.(might cause problems if environment is locked) .
	/// \throw openrave_exception if command doesn't succeed	/// \throw openrave_exception if command doesn't succeed
	virtual int Configure(ConfigureCommand command, bool blocking=false) = 0;	virtual int Configure(ConfigureCommand command, bool blocking=false) = 0;

	/// \brief Simulate one step forward for sensors.	/// \brief Simulate one step forward for sensors.

	skipping to change at line 311	skipping to change at line 354
	/// \param trans - The transform defining the frame of the sensor.	/// \param trans - The transform defining the frame of the sensor.
	virtual void SetTransform(const Transform& trans) = 0;	virtual void SetTransform(const Transform& trans) = 0;
	virtual Transform GetTransform() = 0;	virtual Transform GetTransform() = 0;

	/// \brief Register a callback whenever new sensor data comes in.	/// \brief Register a callback whenever new sensor data comes in.
	/// \param type the sensor type to register for	/// \param type the sensor type to register for
	/// \param callback the user function to call, note that this might blo ck the thread generating/receiving sensor data	/// \param callback the user function to call, note that this might blo ck the thread generating/receiving sensor data
	virtual boost::shared_ptr<void> RegisterDataCallback(SensorType type, c onst boost::function<void(SensorDataConstPtr)>& callback) OPENRAVE_DUMMY_IM PLEMENTATION;	virtual boost::shared_ptr<void> RegisterDataCallback(SensorType type, c onst boost::function<void(SensorDataConstPtr)>& callback) OPENRAVE_DUMMY_IM PLEMENTATION;

	/// \return the name of the sensor	/// \return the name of the sensor

	virtual const std::string& GetName() const { return _name; }	virtual const std::string& GetName() const {
	virtual void SetName(const std::string& newname) { _name = newname; }	return _name;
		}
		virtual void SetName(const std::string& newname) {
		_name = newname;
		}

	/// \deprecated (11/03/28)	/// \deprecated (11/03/28)

	virtual bool Init(const std::string&) RAVE_DEPRECATED { RAVELOG_WARN("S	virtual bool Init(const std::string&) RAVE_DEPRECATED {
	ensorBase::Init has been deprecated\n"); return Configure(CC_PowerOn)>0; }	RAVELOG_WARN("SensorBase::Init has been deprecated\n"); return Conf
	virtual void Reset(int) RAVE_DEPRECATED { RAVELOG_WARN("SensorBase::Res	igure(CC_PowerOn)>0;
	et has been deprecated\n"); Configure(CC_PowerOff); }	}
		virtual void Reset(int) RAVE_DEPRECATED {
		RAVELOG_WARN("SensorBase::Reset has been deprecated\n"); Configure(
		CC_PowerOff);
		}

	protected:	protected:

	std::string _name; ///< name of the sensor	std::string _name; ///< name of the sensor

	private:	private:

	virtual const char* GetHash() const { return OPENRAVE_SENSOR_HASH; }	virtual const char* GetHash() const {
		return OPENRAVE_SENSOR_HASH;
		}
	};	};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	#endif	#endif

End of changes. 36 change blocks.
	168 lines changed or deleted	225 lines changed or added

	sensorsystem.h	sensorsystem.h

	skipping to change at line 19	skipping to change at line 19
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	// GNU Lesser General Public License for more details.	// GNU Lesser General Public License for more details.
	//	//
	// You should have received a copy of the GNU Lesser General Public License	// You should have received a copy of the GNU Lesser General Public License
	// along with this program. If not, see <http://www.gnu.org/licenses/>.	// along with this program. If not, see <http://www.gnu.org/licenses/>.
	/** \file sensorsystem.h	/** \file sensorsystem.h
	\brief Sensor systems used to define how environment information is org anized and how bodies are managed.	\brief Sensor systems used to define how environment information is org anized and how bodies are managed.

	*/	*/
	#ifndef OPENRAVE_SENSORSYSTEM_H	#ifndef OPENRAVE_SENSORSYSTEM_H
	#define OPENRAVE_SENSORSYSTEM_H	#define OPENRAVE_SENSORSYSTEM_H

	namespace OpenRAVE {	namespace OpenRAVE {

	/** \brief <b>[interface]</b> Used to manage the creation and destruction o f bodies. See \ref arch_sensorsystem.	/** \brief <b>[interface]</b> Used to manage the creation and destruction o f bodies. See \ref arch_sensorsystem.
	\ingroup interfaces	\ingroup interfaces

	*/	*/
	class OPENRAVE_API SensorSystemBase : public InterfaceBase	class OPENRAVE_API SensorSystemBase : public InterfaceBase
	{	{
	public:	public:

	SensorSystemBase(EnvironmentBasePtr penv) : InterfaceBase(PT_SensorSyst	SensorSystemBase(EnvironmentBasePtr penv) : InterfaceBase(PT_SensorSyst
	em, penv) {}	em, penv) {
	virtual ~SensorSystemBase() {}	}
		virtual ~SensorSystemBase() {
		}

	/// return the static interface type this class points to (used for saf e casting)	/// return the static interface type this class points to (used for saf e casting)

	static inline InterfaceType GetInterfaceTypeStatic() { return PT_Sensor	static inline InterfaceType GetInterfaceTypeStatic() {
	System; }	return PT_SensorSystem;
		}

	/// resets the system and stops managing all objects. Any objects that are not locked, are deleted	/// resets the system and stops managing all objects. Any objects that are not locked, are deleted
	virtual void Reset() = 0;	virtual void Reset() = 0;

	/// automatically register bodies that have some type of SensorSystem d ata (usually done through xml)	/// automatically register bodies that have some type of SensorSystem d ata (usually done through xml)
	virtual void AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies ) = 0;	virtual void AddRegisteredBodies(const std::vector<KinBodyPtr>& vbodies ) = 0;

	/// add body for registering with sensor system	/// add body for registering with sensor system
	/// pdata is a pointer to a data structor holding tracking/registration information for the system	/// pdata is a pointer to a data structor holding tracking/registration information for the system
	virtual KinBody::ManageDataPtr AddKinBody(KinBodyPtr pbody, XMLReadable ConstPtr pdata) = 0;	virtual KinBody::ManageDataPtr AddKinBody(KinBodyPtr pbody, XMLReadable ConstPtr pdata) = 0;

	skipping to change at line 59	skipping to change at line 63
	virtual bool IsBodyPresent(KinBodyPtr pbody) = 0;	virtual bool IsBodyPresent(KinBodyPtr pbody) = 0;
	/// enable/disable a body from being updated by the sensor system	/// enable/disable a body from being updated by the sensor system
	virtual bool EnableBody(KinBodyPtr pbody, bool bEnable) = 0;	virtual bool EnableBody(KinBodyPtr pbody, bool bEnable) = 0;

	/// switches the registrations of two bodies. Can be used to quickly ch ange the models of the current bodies	/// switches the registrations of two bodies. Can be used to quickly ch ange the models of the current bodies
	/// \param pbody1 First body to switch	/// \param pbody1 First body to switch
	/// \param pbody2 Second body to switch	/// \param pbody2 Second body to switch
	virtual bool SwitchBody(KinBodyPtr pbody1, KinBodyPtr pbody2) = 0;	virtual bool SwitchBody(KinBodyPtr pbody1, KinBodyPtr pbody2) = 0;

	protected:	protected:

	virtual void SetManageData(KinBodyPtr pbody, KinBody::ManageDataPtr data) {	virtual void SetManageData(KinBodyPtr pbody, KinBody::ManageDataPtr dat a) {
	pbody->SetManageData(data);	pbody->SetManageData(data);

	}	}

	private:	private:

	virtual const char* GetHash() const { return OPENRAVE_SENSORSYSTEM_HASH	virtual const char* GetHash() const {
	; }	return OPENRAVE_SENSORSYSTEM_HASH;
		}
	};	};

	/// A very simple sensor system example that manages raw detection data	/// A very simple sensor system example that manages raw detection data
	class OPENRAVE_API SimpleSensorSystem : public SensorSystemBase	class OPENRAVE_API SimpleSensorSystem : public SensorSystemBase
	{	{
	public:	public:
	class OPENRAVE_API XMLData : public XMLReadable {	class OPENRAVE_API XMLData : public XMLReadable {

	public:	public:
	XMLData(const std::string& xmlid) : XMLReadable(xmlid), id(0) {}	XMLData(const std::string& xmlid) : XMLReadable(xmlid), id(0) {
	virtual void copy(boost::shared_ptr<XMLData const> pdata) { *this =	}
	*pdata; }	virtual void copy(boost::shared_ptr<XMLData const> pdata) {
		this = pdata;
		}


	std::string sid; ///< global id for the system id	std::string sid; ///< global id for the system id
	int id;	int id;

	std::string strOffsetLink; ///< the link where the markers are atta	std::string strOffsetLink; ///< the link where the markers
	ched (if any)	are attached (if any)
	Transform transOffset,transPreOffset; // final offset = transOffset	Transform transOffset,transPreOffset; // final offset = tra
	* transReturnedFromVision * transPreOffset	nsOffset * transReturnedFromVision * transPreOffset

	friend class SimpleSensorSystem;	friend class SimpleSensorSystem;
	};	};

	class OPENRAVE_API BodyData : public KinBody::ManageData {	class OPENRAVE_API BodyData : public KinBody::ManageData {

	public:	public:
	BodyData(SensorSystemBasePtr psensorsystem, KinBodyPtr pbody, boost::sh	BodyData(SensorSystemBasePtr psensorsystem, KinBodyPtr pbody, boost
	ared_ptr<XMLData> initdata) : KinBody::ManageData(psensorsystem), _initdata	::shared_ptr<XMLData> initdata) : KinBody::ManageData(psensorsystem), _init
	(initdata), bPresent(false), bEnabled(true), bLock(false)	data(initdata), bPresent(false), bEnabled(true), bLock(false)
	{	{
	SetBody(pbody);	SetBody(pbody);
	}	}


	virtual XMLReadableConstPtr GetData() const { return _initdata; }	virtual XMLReadableConstPtr GetData() const {
	virtual KinBody::LinkPtr GetOffsetLink() const { return KinBody::Li	return _initdata;
	nkPtr(_plink); }	}
		virtual KinBody::LinkPtr GetOffsetLink() const {
		return KinBody::LinkPtr(_plink);
		}


	virtual bool IsPresent() const { return bPresent; }	virtual bool IsPresent() const {
	virtual bool IsEnabled() const { return bEnabled; }	return bPresent;
	virtual bool IsLocked() const { return bLock; }	}
	virtual bool Lock(bool bDoLock) { bLock = bDoLock; return true; }	virtual bool IsEnabled() const {
		return bEnabled;
		}
		virtual bool IsLocked() const {
		return bLock;
		}
		virtual bool Lock(bool bDoLock) {
		bLock = bDoLock; return true;
		}


	virtual int GetId() { return _initdata->id; }	virtual int GetId() {
	virtual const std::string& GetSid() { return _initdata->sid; }	return _initdata->id;
	virtual const Transform& GetRecentTransform() { return tnew; }	}
		virtual const std::string& GetSid() {
		return _initdata->sid;
		}
		virtual const Transform& GetRecentTransform() {
		return tnew;
		}


	protected:	protected:
	virtual void SetBody(KinBodyPtr pbody)	virtual void SetBody(KinBodyPtr pbody)
	{	{
	KinBody::LinkPtr plink;	KinBody::LinkPtr plink;
	if( _initdata->strOffsetLink.size() > 0 )	if( _initdata->strOffsetLink.size() > 0 )
	plink = pbody->GetLink(_initdata->strOffsetLink);	plink = pbody->GetLink(_initdata->strOffsetLink);
	if( !plink )	if( !plink )
	plink = pbody->GetLinks().front();	plink = pbody->GetLinks().front();
	_plink = plink;	_plink = plink;
	}	}

	boost::shared_ptr<XMLData> _initdata;	boost::shared_ptr<XMLData> _initdata;
	uint64_t lastupdated;	uint64_t lastupdated;

	Transform tnew; ///< most recent transform that is was set	Transform tnew; ///< most recent transform that is was set

	bool bPresent;	bool bPresent;
	bool bEnabled;	bool bEnabled;
	bool bLock;	bool bLock;

	KinBody::LinkWeakPtr _plink;	KinBody::LinkWeakPtr _plink;
	friend class SimpleSensorSystem;	friend class SimpleSensorSystem;
	};	};

	class OPENRAVE_API SimpleXMLReader : public BaseXMLReader	class OPENRAVE_API SimpleXMLReader : public BaseXMLReader
	{	{

	public:	public:
	SimpleXMLReader(boost::shared_ptr<XMLData>);	SimpleXMLReader(boost::shared_ptr<XMLData>);

	virtual XMLReadablePtr GetReadable() { return _pdata; }	virtual XMLReadablePtr GetReadable() {
		return _pdata;
		}
	virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);	virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
	virtual bool endElement(const std::string& name);	virtual bool endElement(const std::string& name);
	virtual void characters(const std::string& ch);	virtual void characters(const std::string& ch);


	protected:	protected:
	boost::shared_ptr<XMLData> _pdata;	boost::shared_ptr<XMLData> _pdata;
	std::stringstream ss;	std::stringstream ss;
	};	};

	/// registers the XML reader, do not call in the constructor of this cl ass!	/// registers the XML reader, do not call in the constructor of this cl ass!
	static boost::shared_ptr<void> RegisterXMLReaderId(EnvironmentBasePtr p env, const std::string& xmlid);	static boost::shared_ptr<void> RegisterXMLReaderId(EnvironmentBasePtr p env, const std::string& xmlid);

	SimpleSensorSystem(const std::string& xmlid, EnvironmentBasePtr penv);	SimpleSensorSystem(const std::string& xmlid, EnvironmentBasePtr penv);
	virtual ~SimpleSensorSystem();	virtual ~SimpleSensorSystem();


	skipping to change at line 163	skipping to change at line 192
	virtual bool EnableBody(KinBodyPtr pbody, bool bEnable);	virtual bool EnableBody(KinBodyPtr pbody, bool bEnable);
	virtual bool SwitchBody(KinBodyPtr pbody1, KinBodyPtr pbody2);	virtual bool SwitchBody(KinBodyPtr pbody1, KinBodyPtr pbody2);

	protected:	protected:
	typedef std::pair<boost::shared_ptr<BodyData>, Transform > SNAPSHOT;	typedef std::pair<boost::shared_ptr<BodyData>, Transform > SNAPSHOT;
	typedef std::map<int,boost::shared_ptr<BodyData> > BODIES;	typedef std::map<int,boost::shared_ptr<BodyData> > BODIES;
	virtual boost::shared_ptr<BodyData> CreateBodyData(KinBodyPtr pbody, bo ost::shared_ptr<XMLData const> pdata);	virtual boost::shared_ptr<BodyData> CreateBodyData(KinBodyPtr pbody, bo ost::shared_ptr<XMLData const> pdata);
	virtual void _UpdateBodies(std::list<SNAPSHOT>& listbodies);	virtual void _UpdateBodies(std::list<SNAPSHOT>& listbodies);
	virtual void _UpdateBodiesThread();	virtual void _UpdateBodiesThread();


	virtual void SetRecentTransform(boost::shared_ptr<BodyData> pdata, cons	virtual void SetRecentTransform(boost::shared_ptr<BodyData> pdata, cons
	t Transform& t) { pdata->tnew = t; }	t Transform& t) {
		pdata->tnew = t;
		}

	/// creates a reader to parse the data	/// creates a reader to parse the data
	static BaseXMLReaderPtr CreateXMLReaderId(const std::string& xmlid, Int erfaceBasePtr ptr, const AttributesList& atts);	static BaseXMLReaderPtr CreateXMLReaderId(const std::string& xmlid, Int erfaceBasePtr ptr, const AttributesList& atts);

	std::string _xmlid;	std::string _xmlid;
	BODIES _mapbodies;	BODIES _mapbodies;
	boost::mutex _mutex;	boost::mutex _mutex;

	uint64_t _expirationtime; ///< expiration time in us	uint64_t _expirationtime; ///< expiration time in us
	bool _bShutdown;	bool _bShutdown;
	boost::thread _threadUpdate;	boost::thread _threadUpdate;
	};	};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	#endif	#endif

End of changes. 21 change blocks.
	42 lines changed or deleted	69 lines changed or added

	trajectory.h	trajectory.h

	skipping to change at line 28	skipping to change at line 28
	\brief Define a time-parameterized trajectory of robot configurations.	\brief Define a time-parameterized trajectory of robot configurations.
	*/	*/

	#ifndef TRAJECTORY_H	#ifndef TRAJECTORY_H
	#define TRAJECTORY_H	#define TRAJECTORY_H

	namespace OpenRAVE {	namespace OpenRAVE {

	/** \brief <b>[interface]</b> Encapsulate a time-parameterized trajectories of robot configurations. See \ref arch_trajectory.	/** \brief <b>[interface]</b> Encapsulate a time-parameterized trajectories of robot configurations. See \ref arch_trajectory.
	\ingroup interfaces	\ingroup interfaces

	*/	*/
	class OPENRAVE_API TrajectoryBase : public InterfaceBase	class OPENRAVE_API TrajectoryBase : public InterfaceBase
	{	{
	public:	public:
	/// \brief trajectory interpolation and sampling methods	/// \brief trajectory interpolation and sampling methods
	enum InterpEnum {	enum InterpEnum {

	NONE=0, ///< unspecified timing info	NONE=0, ///< unspecified timing info
	LINEAR=1, ///< linear interpolation	LINEAR=1, ///< linear interpolation
	LINEAR_BLEND=2, ///< linear with quadratic blends	LINEAR_BLEND=2, ///< linear with quadratic blends
	CUBIC=3, ///< cubic spline interpolation	CUBIC=3, ///< cubic spline interpolation
	QUINTIC=4, ///< quintic min-jerk interpolation	QUINTIC=4, ///< quintic min-jerk interpolation
	NUM_METHODS=5, ///< number of interpolation methods	NUM_METHODS=5, ///< number of interpolation methods
	};	};

	/// \brief options for serializing trajectories	/// \brief options for serializing trajectories
	enum TrajectoryOptions {	enum TrajectoryOptions {

	TO_OneLine = 1, ///< if set, will write everything without newlines	TO_OneLine = 1, ///< if set, will write everything without newl
	, otherwise	ines, otherwise
	///< will start a newline for the header and every	///< will start a newline for the header and ev
	trajectory point	ery trajectory point
	TO_NoHeader = 2, ///< do not write the header that specifies number	TO_NoHeader = 2, ///< do not write the header that specifies nu
	of points, degrees of freedom, and other options	mber of points, degrees of freedom, and other options
	TO_IncludeTimestamps = 4,	TO_IncludeTimestamps = 4,
	TO_IncludeBaseTransformation = 8,	TO_IncludeBaseTransformation = 8,

	TO_IncludeVelocities = 0x10, ///< include velocities. If TO_Include	TO_IncludeVelocities = 0x10, ///< include velocities. If TO_Inc
	BaseTransformation is also set, include the base	ludeBaseTransformation is also set, include the base
	///< base link velocity in terms of line	///< base link velocity in terms o
	ar and angular velocity	f linear and angular velocity
	TO_IncludeTorques = 0x20, ///< include torques	TO_IncludeTorques = 0x20, ///< include torques
	TO_InterpolationMask = 0x1c0, ///< bits to store the interpolation	TO_InterpolationMask = 0x1c0, ///< bits to store the interpolat
	information	ion information
	};	};

	/// Via point along the trajectory (joint configuration with a timestam p)	/// Via point along the trajectory (joint configuration with a timestam p)
	class TPOINT	class TPOINT
	{	{

	public:	public:
	TPOINT() : time(0), blend_radius(0) {}	TPOINT() : time(0), blend_radius(0) {
	TPOINT(const std::vector<dReal>& newq, dReal newtime) : time(newtim	}
	e), blend_radius(0) { q = newq; }	TPOINT(const std::vector<dReal>& newq, dReal newtime) : time(newtim
	TPOINT(const std::vector<dReal>& newq, const Transform& newtrans, d	e), blend_radius(0) {
	Real newtime) : time(newtime), blend_radius(0) { q = newq; trans = newtrans	q = newq;
	; }	}
		TPOINT(const std::vector<dReal>& newq, const Transform& newtrans, d
		Real newtime) : time(newtime), blend_radius(0) {
		q = newq; trans = newtrans;
		}


	enum TPcomponent { POS=0, //!< joint angle position	enum TPcomponent { POS=0, //!< joint angle position
	VEL, //!< joint angle velocity	VEL, //!< joint angle velocity
	ACC, //!< joint angle acceleration	ACC, //!< joint angle acceleration
	NUM_COMPONENTS };	NUM_COMPONENTS };

	friend std::ostream& operator<<(std::ostream& O, const TPOINT& tp);	friend std::ostream& operator<<(std::ostream& O, const TPOINT& tp);
	void Setq(std::vector<dReal>* values)	void Setq(std::vector<dReal>* values)
	{	{
	assert(values->size() == q.size());	assert(values->size() == q.size());
	for(int i = 0; i < (int)values->size(); i++)	for(int i = 0; i < (int)values->size(); i++)
	q[i] = values->at(i);	q[i] = values->at(i);

	// reset the blend_radius	// reset the blend_radius
	blend_radius=0;	blend_radius=0;

	}	}


	Transform trans; ///< transform of the first link	Transform trans; ///< transform of the first lin
	Vector linearvel; ///< instanteneous linear velocity	k
	Vector angularvel; ///< instanteneous angular velocity	Vector linearvel; ///< instanteneous linear veloc
	std::vector<dReal> q; ///< joint configuration	ity
	std::vector<dReal> qdot; ///< instantaneous joint velocities	Vector angularvel; ///< instanteneous angular velo
	std::vector<dReal> qtorque; ///< feedforward torque [optional]	city
	dReal time; ///< time stamp of trajectory point	std::vector<dReal> q; ///< joint configuration
	dReal blend_radius;	std::vector<dReal> qdot; ///< instantaneous joint veloci
		ties
		std::vector<dReal> qtorque; ///< feedforward torque [option
		al]
		dReal time; ///< time stamp of trajectory p
		oint
		dReal blend_radius;
	};	};

	class TSEGMENT	class TSEGMENT
	{	{

	public:	public:
	//! the different segment types	//! the different segment types

	enum Type { START=0, //!< starting trajectory segment	enum Type { START=0, //!< starting trajectory segment
	MIDDLE, //!< middle trajectory segment	MIDDLE, //!< middle trajectory segment
	END, //!< ending trajectory segment	END, //!< ending trajectory segment
	NUM_TYPES };	NUM_TYPES };


	void SetDimensions(int curve_degree, int num_dof) { coeff.resize((c	void SetDimensions(int curve_degree, int num_dof) {
	urve_degree+1)*num_dof); _curvedegrees = curve_degree; }	coeff.resize((curve_degree+1)*num_dof); _curvedegrees = curve_d
		egree;
		}


	inline dReal Get(int deg, int dof) const { return coeff[dof*(_curve	inline dReal Get(int deg, int dof) const {
	degrees+1)+deg]; }	return coeff[dof*(_curvedegrees+1)+deg];
	dReal& Get(int deg, int dof) { return coeff[dof*(_curvedegrees+1)+d	}
	eg]; }	dReal& Get(int deg, int dof) {
		return coeff[dof*(_curvedegrees+1)+deg];
		}

	friend std::ostream& operator<<(std::ostream& O, const TSEGMENT& tp );	friend std::ostream& operator<<(std::ostream& O, const TSEGMENT& tp );


	Vector linearvel; ///< instanteneous linear velocity	Vector linearvel; ///< instanteneous linear veloc
	Vector angularvel; ///< instanteneous angular velocity	ity
		Vector angularvel; ///< instanteneous angular velo
		city


	private:	private:
	dReal _fduration;	dReal _fduration;
	int _curvedegrees;	int _curvedegrees;

	std::vector<dReal> coeff; ///< num_degrees x num_dof coeffici ents of the segment	std::vector<dReal> coeff; ///< num_degrees x num_dof coefficients of the segment

	friend class TrajectoryBase;	friend class TrajectoryBase;
	};	};

	TrajectoryBase(EnvironmentBasePtr penv, int nDOF);	TrajectoryBase(EnvironmentBasePtr penv, int nDOF);

	virtual ~TrajectoryBase() {}	virtual ~TrajectoryBase() {
		}

	/// return the static interface type this class points to (used for saf e casting)	/// return the static interface type this class points to (used for saf e casting)

	static inline InterfaceType GetInterfaceTypeStatic() { return PT_Trajec	static inline InterfaceType GetInterfaceTypeStatic() {
	tory; }	return PT_Trajectory;
		}

	/// clears all points and resets the dof of the trajectory	/// clears all points and resets the dof of the trajectory
	virtual void Reset(int nDOF);	virtual void Reset(int nDOF);

	/// getting information about the trajectory	/// getting information about the trajectory

	inline dReal GetTotalDuration() const { return _vecpoints.size() >	inline dReal GetTotalDuration() const {
	0 ? _vecpoints.back().time : 0; }	return _vecpoints.size() > 0 ? _vecpoints.back().time : 0;
	InterpEnum GetInterpMethod() const { return _interpMethod; }	}
	const std::vector<TrajectoryBase::TPOINT>& GetPoints() const { return _	InterpEnum GetInterpMethod() const {
	vecpoints; }	return _interpMethod;
	std::vector<TrajectoryBase::TPOINT>& GetPoints() { return _vecpoints; }	}
	const std::vector<TrajectoryBase::TSEGMENT>& GetSegments() const { retu	const std::vector<TrajectoryBase::TPOINT>& GetPoints() const {
	rn _vecsegments; }	return _vecpoints;
		}
		std::vector<TrajectoryBase::TPOINT>& GetPoints() {
		return _vecpoints;
		}
		const std::vector<TrajectoryBase::TSEGMENT>& GetSegments() const {
		return _vecsegments;
		}

	virtual void Clear();	virtual void Clear();

	/// add a point to the trajectory	/// add a point to the trajectory

	virtual void AddPoint(const TrajectoryBase::TPOINT& p) { assert( _nDOF	virtual void AddPoint(const TrajectoryBase::TPOINT& p) {
	== (int)p.q.size()); _vecpoints.push_back(p); }	assert( _nDOF == (int)p.q.size()); _vecpoints.push_back(p);
		}

	/** \brief Preprocesses the trajectory for later sampling and set its i nterpolation method.	/** \brief Preprocesses the trajectory for later sampling and set its i nterpolation method.

	\param[in] robot [optional] robot to do the timing for	\param[in] robot [optional] robot to do the timing for
	\param[in] interpolationMethod method to use for interpolation	\param[in] interpolationMethod method to use for interpolation
	\param bAutoCalcTiming If true, will retime the trajectory using ma ximum joint velocities and accelerations, otherwise it expects the time sta mps of each point to be set.	\param bAutoCalcTiming If true, will retime the trajectory using ma ximum joint velocities and accelerations, otherwise it expects the time sta mps of each point to be set.
	\param[in] bActiveDOFs If true, then the trajectory is specified in the robot's active degrees of freedom	\param[in] bActiveDOFs If true, then the trajectory is specified in the robot's active degrees of freedom
	and the maximum velocities and accelerations will be extracted appr opriately. Affine transformations	and the maximum velocities and accelerations will be extracted appr opriately. Affine transformations
	are ignored in the retiming if true. If false, then use the	are ignored in the retiming if true. If false, then use the
	robot's full joint configuration and affine transformation for max velocities.	robot's full joint configuration and affine transformation for max velocities.
	\param[in] fMaxVelMult The percentage of the max velocity of each j oint to use when retiming.	\param[in] fMaxVelMult The percentage of the max velocity of each j oint to use when retiming.

	*/	*/
	virtual bool CalcTrajTiming(RobotBaseConstPtr robot, InterpEnum interpo lationMethod, bool bAutoCalcTiming, bool bActiveDOFs, dReal fMaxVelMult=1);	virtual bool CalcTrajTiming(RobotBaseConstPtr robot, InterpEnum interpo lationMethod, bool bAutoCalcTiming, bool bActiveDOFs, dReal fMaxVelMult=1);

	/// \deprecated (11/06/14) see planningutils::ValidateTrajectory	/// \deprecated (11/06/14) see planningutils::ValidateTrajectory
	virtual bool IsValid() const RAVE_DEPRECATED;	virtual bool IsValid() const RAVE_DEPRECATED;

	/// tests if a point violates any position, velocity or accel constrain ts	/// tests if a point violates any position, velocity or accel constrain ts
	//virtual bool IsValidPoint(const TPOINT& tp) const;	//virtual bool IsValidPoint(const TPOINT& tp) const;

	/// \brief Sample the trajectory at the given time using the current in terpolation method.	/// \brief Sample the trajectory at the given time using the current in terpolation method.

	virtual bool SampleTrajectory(dReal time, TrajectoryBase::TPOINT &samp le) const;	virtual bool SampleTrajectory(dReal time, TrajectoryBase::TPOINT &sampl e) const;

	/// Write to a stream, see TrajectoryOptions for file format	/// Write to a stream, see TrajectoryOptions for file format
	/// \param sinput stream to read the data from	/// \param sinput stream to read the data from
	/// \param options a combination of enums in TrajectoryOptions	/// \param options a combination of enums in TrajectoryOptions
	virtual bool Write(std::ostream& sinput, int options) const;	virtual bool Write(std::ostream& sinput, int options) const;

	/// Reads the trajectory, expects the filename to have a header.	/// Reads the trajectory, expects the filename to have a header.
	/// \param sout stream to output the trajectory data	/// \param sout stream to output the trajectory data
	/// \param robot The robot to attach the trajrectory to, if specified, will	/// \param robot The robot to attach the trajrectory to, if specified, will
	/// call CalcTrajTiming to get the correct trajectory velo cities.	/// call CalcTrajTiming to get the correct trajectory velo cities.
	virtual bool Read(std::istream& sout, RobotBasePtr robot);	virtual bool Read(std::istream& sout, RobotBasePtr robot);

	virtual bool Read(const std::string& filename, RobotBasePtr robot) RAVE _DEPRECATED;	virtual bool Read(const std::string& filename, RobotBasePtr robot) RAVE _DEPRECATED;
	virtual bool Write(const std::string& filename, int options) const RAVE _DEPRECATED;	virtual bool Write(const std::string& filename, int options) const RAVE _DEPRECATED;


	virtual int GetDOF() const { return _nDOF; }	virtual int GetDOF() const {
		return _nDOF;
		}
	private:	private:

	/// \brief Linear interpolation using the maximum joint velocities for timing.	/// \brief Linear interpolation using the maximum joint velocities for timing.
	virtual bool _SetLinear(bool bAutoCalcTiming, bool bActiveDOFs);	virtual bool _SetLinear(bool bAutoCalcTiming, bool bActiveDOFs);

	//// linear interpolation with parabolic blends	//// linear interpolation with parabolic blends
	//virtual bool _SetLinearBlend(bool bAutoCalcTiming);	//virtual bool _SetLinearBlend(bool bAutoCalcTiming);

	/// calculate the coefficients of a the parabolic and linear blends	/// calculate the coefficients of a the parabolic and linear blends
	/// with continuous endpoint positions and velocities for via points.	/// with continuous endpoint positions and velocities for via points.
	//virtual void _CalculateLinearBlendCoefficients(TSEGMENT::Type segType ,TSEGMENT& seg, TSEGMENT& prev,TPOINT& p0, TPOINT& p1,const dReal blendAcce l[]);	//virtual void _CalculateLinearBlendCoefficients(TSEGMENT::Type segType ,TSEGMENT& seg, TSEGMENT& prev,TPOINT& p0, TPOINT& p1,const dReal blendAcce l[]);

	/// cubic spline interpolation	/// cubic spline interpolation
	virtual bool _SetCubic(bool bAutoCalcTiming, bool bActiveDOFs);	virtual bool _SetCubic(bool bAutoCalcTiming, bool bActiveDOFs);

	/// calculate the coefficients of a smooth cubic spline with	/// calculate the coefficients of a smooth cubic spline with
	/// continuous endpoint positions and velocities for via points.	/// continuous endpoint positions and velocities for via points.

	virtual void _CalculateCubicCoefficients(TrajectoryBase::TSEGMENT& , co nst TrajectoryBase::TPOINT& tp0, const TrajectoryBase::TPOINT& tp1);	virtual void _CalculateCubicCoefficients(TrajectoryBase::TSEGMENT&, con st TrajectoryBase::TPOINT& tp0, const TrajectoryBase::TPOINT& tp1);

	//bool _SetQuintic(bool bAutoCalcTiming, bool bActiveDOFs);	//bool _SetQuintic(bool bAutoCalcTiming, bool bActiveDOFs);

	/// calculate the coefficients of a smooth quintic spline with	/// calculate the coefficients of a smooth quintic spline with
	/// continuous endpoint positions and velocities for via points	/// continuous endpoint positions and velocities for via points
	/// using minimum jerk heuristics	/// using minimum jerk heuristics
	//void _CalculateQuinticCoefficients(TSEGMENT&, TPOINT& tp0, TPOINT& tp 1);	//void _CalculateQuinticCoefficients(TSEGMENT&, TPOINT& tp0, TPOINT& tp 1);

	/// recalculate all via point velocities and accelerations	/// recalculate all via point velocities and accelerations
	virtual void _RecalculateViaPointDerivatives();	virtual void _RecalculateViaPointDerivatives();

	skipping to change at line 244	skipping to change at line 272
	virtual bool _SampleCubic(const TrajectoryBase::TPOINT& p0, const Traje ctoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, Tra jectoryBase::TPOINT& sample) const;	virtual bool _SampleCubic(const TrajectoryBase::TPOINT& p0, const Traje ctoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, Tra jectoryBase::TPOINT& sample) const;

	/// \brief Sample the trajectory using quintic interpolation with minim um jerk.	/// \brief Sample the trajectory using quintic interpolation with minim um jerk.
	virtual bool _SampleQuintic(const TrajectoryBase::TPOINT& p0, const Tra jectoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, T rajectoryBase::TPOINT& sample) const;	virtual bool _SampleQuintic(const TrajectoryBase::TPOINT& p0, const Tra jectoryBase::TPOINT& p1, const TrajectoryBase::TSEGMENT& seg, dReal time, T rajectoryBase::TPOINT& sample) const;

	std::vector<TrajectoryBase::TPOINT> _vecpoints;	std::vector<TrajectoryBase::TPOINT> _vecpoints;
	std::vector<TSEGMENT> _vecsegments;	std::vector<TSEGMENT> _vecsegments;
	std::vector<dReal> _lowerJointLimit, _upperJointLimit, _maxJointVel, _m axJointAccel;	std::vector<dReal> _lowerJointLimit, _upperJointLimit, _maxJointVel, _m axJointAccel;
	Vector _maxAffineTranslationVel;	Vector _maxAffineTranslationVel;
	dReal _maxAffineRotationQuatVel;	dReal _maxAffineRotationQuatVel;

	int _nQuaternionIndex; ///< the index of a quaternion rotation, if one exists (interpolation is different for quaternions)	int _nQuaternionIndex; ///< the index of a quaternion rotation, if one exists (interpolation is different for quaternions)

	/// computes the difference of two states necessary for correct interpo lation when there are circular joints. Default is regular subtraction.	/// computes the difference of two states necessary for correct interpo lation when there are circular joints. Default is regular subtraction.
	/// _diffstatefn(q1,q2) -> q1 -= q2	/// _diffstatefn(q1,q2) -> q1 -= q2
	boost::function<void(std::vector<dReal>&,const std::vector<dReal>&)> _d iffstatefn;	boost::function<void(std::vector<dReal>&,const std::vector<dReal>&)> _d iffstatefn;


	InterpEnum _interpMethod;	InterpEnum _interpMethod;
	int _nDOF;	int _nDOF;


	virtual const char* GetHash() const { return OPENRAVE_TRAJECTORY_HASH;	virtual const char* GetHash() const {
	}	return OPENRAVE_TRAJECTORY_HASH;
		}
	};	};

	typedef TrajectoryBase Trajectory;	typedef TrajectoryBase Trajectory;

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	#endif // TRAJECTORY_H	#endif // TRAJECTORY_H

End of changes. 26 change blocks.
	77 lines changed or deleted	106 lines changed or added

This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/

	interfacehashes.h	interfacehashes.h

	skipping to change at line 12	skipping to change at line 12
	#define OPENRAVE_ROBOT_HASH "bee29611dc861971dc759376933d69cf"	#define OPENRAVE_ROBOT_HASH "bee29611dc861971dc759376933d69cf"
	#define OPENRAVE_PLANNER_HASH "7b8864032c6bf5c572e0535c9c164fd4"	#define OPENRAVE_PLANNER_HASH "7b8864032c6bf5c572e0535c9c164fd4"
	#define OPENRAVE_KINBODY_HASH "2d1499ecb721b3af313da4c26b836ce2"	#define OPENRAVE_KINBODY_HASH "2d1499ecb721b3af313da4c26b836ce2"
	#define OPENRAVE_SENSORSYSTEM_HASH "43920009ad109091d7e59157a5af3a24"	#define OPENRAVE_SENSORSYSTEM_HASH "43920009ad109091d7e59157a5af3a24"
	#define OPENRAVE_CONTROLLER_HASH "3ff86056459c72cbdb34af28b98f0992"	#define OPENRAVE_CONTROLLER_HASH "3ff86056459c72cbdb34af28b98f0992"
	#define OPENRAVE_MODULE_HASH "91a6adfcb28019207938af3fbbf759b3"	#define OPENRAVE_MODULE_HASH "91a6adfcb28019207938af3fbbf759b3"
	#define OPENRAVE_IKSOLVER_HASH "721e42b775ddd137a81e53f69519d1d9"	#define OPENRAVE_IKSOLVER_HASH "721e42b775ddd137a81e53f69519d1d9"
	#define OPENRAVE_PHYSICSENGINE_HASH "31b1aa40bf4c5b63aa57c5552bf62d81"	#define OPENRAVE_PHYSICSENGINE_HASH "31b1aa40bf4c5b63aa57c5552bf62d81"
	#define OPENRAVE_SENSOR_HASH "c7227d59d84d6ad880ea5ae166cc087a"	#define OPENRAVE_SENSOR_HASH "c7227d59d84d6ad880ea5ae166cc087a"
	#define OPENRAVE_TRAJECTORY_HASH "260138467755d029a252ef128993907e"	#define OPENRAVE_TRAJECTORY_HASH "260138467755d029a252ef128993907e"

	#define OPENRAVE_VIEWER_HASH "81042bd863f0e42be9a5b96fcc912db3"	#define OPENRAVE_VIEWER_HASH "5770b43a6028630f89678e29a65ee275"
	#define OPENRAVE_SPACESAMPLER_HASH "8d3866c955b9587aaf42e277edd2da2a"	#define OPENRAVE_SPACESAMPLER_HASH "8d3866c955b9587aaf42e277edd2da2a"
	#define OPENRAVE_ENVIRONMENT_HASH "f29fda952d7a5cef7b8b46a7e4f70a27"	#define OPENRAVE_ENVIRONMENT_HASH "f29fda952d7a5cef7b8b46a7e4f70a27"
	#define OPENRAVE_PLUGININFO_HASH "202a84af5eb50775aa5da86956de73fe"	#define OPENRAVE_PLUGININFO_HASH "202a84af5eb50775aa5da86956de73fe"

End of changes. 1 change blocks.
	1 lines changed or deleted	1 lines changed or added