#include?
#include?
#include?
#include?
using?namespace?std;

#ifdef?WIN32
#include?
#endif

#include?
#include?
#include?
#include?

#include?“Jacobian.h“

void?Arrow（const?VectorR3&?tail?const?VectorR3&?head）;

extern?RestPositionOn;
extern?VectorR3?target[];

//?Optimal?damping?values?have?to?be?determined?in?an?ad?hoc?manner??（Yuck!）
//?const?double?Jacobian::DefaultDampingLambda?=?0.6;		//?Optimal?for?the?“Y“?shape?（any?lower?gives?jitter）
const?double?Jacobian::DefaultDampingLambda?=?1.1;			//?Optimal?for?the?DLS?“double?Y“?shape?（any?lower?gives?jitter）
//?const?double?Jacobian::DefaultDampingLambda?=?0.7;			//?Optimal?for?the?DLS?“double?Y“?shape?with?distance?clamping?（lower?gives?jitter）

const?double?Jacobian::PseudoInverseThresholdFactor?=?0.01;
const?double?Jacobian::MaxAngleJtranspose?=?30.0*DegreesToRadians;
const?double?Jacobian::MaxAnglePseudoinverse?=?5.0*DegreesToRadians;
const?double?Jacobian::MaxAngleDLS?=?45.0*DegreesToRadians;
const?double?Jacobian::MaxAngleSDLS?=?45.0*DegreesToRadians;
const?double?Jacobian::baseMaxTargetDist?=?0.4;

Jacobian::Jacobian（Tree*?tree）
{
	Jacobian::tree?=?tree;
	nEffector?=?tree->GetNumEffector（）;
	nJoint?=?tree->GetNumJoint（）;
	nRow?=?3?*?nEffector;
	nCol?=?nJoint;

	Jend.SetSize（nRow?nCol）;				//?The?Jocobian?matrix
	Jend.SetZero（）;
	Jtarget.SetSize（nRow?nCol）;			//?The?Jacobian?matrix?based?on?target?positions
	Jtarget.SetZero（）;
	SetJendActive（）;

	U.SetSize（nRow?nRow）;				//?The?U?matrix?for?SVD?calculations
	w?.SetLength（Min（nRow?nCol））;
	V.SetSize（nCol?nCol）;				//?The?V?matrix?for?SVD?calculations

	dS.SetLength（nRow）;			//?（Target?positions）?-?（End?effector?positions）
	dTheta.SetLength（nCol）;		//?Changes?in?joint?angles
	dPreTheta.SetLength（nCol）;

	//?Used?by?Jacobian?transpose?method?&?DLS?&?SDLS
	dT.SetLength（nRow）;			//?Linearized?change?in?end?effector?positions?based?on?dTheta

	//?Used?by?the?Selectively?Damped?Least?Squares?Method
	//dT.SetLength（nRow）;
	dSclamp.SetLength（nEffector）;
	errorArray.SetLength（nEffector）;
	Jnorms.SetSize（nEffector?nCol）;		//?Holds?the?norms?of?the?active?J?matrix

	Reset（）;
}

void?Jacobian::Reset（）?
{
	//?Used?by?Damped?Least?Squares?Method
	DampingLambda?=?DefaultDampingLambda;
	DampingLambdaSq?=?Square（DampingLambda）;
	//?DampingLambdaSDLS?=?1.5*DefaultDampingLambda;
	
	dSclamp.Fill（HUGE_VAL）;
}

//?Compute?the?deltaS?vector?dS?（the?error?in?end?effector?positions
//?Compute?the?J?and?K?matrices?（the?Jacobians）
void?Jacobian::ComputeJacobian（）?
{
	//?Traverse?tree?to?find?all?end?effectors
	VectorR3?temp;
	double?value;
	Node*?n?=?tree->GetRoot（）;
	while?（?n?）?{	
		if?（?n->IsEffector（）?）?{
			int?i?=?n->GetEffectorNum（）;
			const?VectorR3&?targetPos?=?target[i];

			//?Compute?the?delta?S?value?（differences?from?end?effectors?to?target?positions.
			temp?=?

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????2517??2007-10-19?20:51??IkMethods\Debug\BuildLog.htm

?????文件?????116736??2004-08-04?10:25??IkMethods\Debug\glu32.dll

?????文件?????237568??2004-08-04?10:25??IkMethods\Debug\glut32.dll

?????文件?????389120??2007-10-19?20:51??IkMethods\Debug\IkMethods.exe

?????文件????1522792??2007-10-19?20:51??IkMethods\Debug\IkMethods.ilk

?????文件????1477632??2007-10-19?20:51??IkMethods\Debug\IkMethods.pdb

?????文件?????227525??2007-05-22?21:36??IkMethods\Debug\Jacobian.obj

?????文件??????45294??2007-05-16?15:50??IkMethods\Debug\LinearR2.obj

?????文件?????117493??2007-05-16?15:50??IkMethods\Debug\LinearR3.obj

?????文件?????103758??2007-05-16?15:50??IkMethods\Debug\LinearR4.obj

?????文件?????264740??2007-10-19?20:51??IkMethods\Debug\Main.obj

?????文件?????122404??2007-05-16?15:50??IkMethods\Debug\MatrixRmn.obj

?????文件?????159618??2007-05-16?15:50??IkMethods\Debug\Misc.obj

?????文件?????160848??2007-05-19?14:22??IkMethods\Debug\Node.obj

?????文件??????30378??2007-05-16?15:50??IkMethods\Debug\RgbImage.obj

?????文件??????61307??2007-05-19?14:22??IkMethods\Debug\Tree.obj

?????文件????1002496??2007-10-19?20:51??IkMethods\Debug\vc70.idb

?????文件?????282624??2007-10-19?20:51??IkMethods\Debug\vc70.pdb

?????文件??????36840??2007-05-16?15:50??IkMethods\Debug\VectorRn.obj

?????文件?????389120??2007-05-20?08:53??IkMethods\Debug\復件?IkMethods.exe

?????文件??????74528??2005-08-10?10:19??IkMethods\GL\GL.h

?????文件??????12024??2005-08-10?10:19??IkMethods\GL\GLAux.h

?????文件?????203244??2005-08-10?10:19??IkMethods\GL\glext.h

?????文件??????18282??2005-08-10?10:19??IkMethods\GL\GLU.h

?????文件??????96289??2006-07-06?17:42??IkMethods\GL\glui.h

?????文件??????21494??2005-08-10?10:19??IkMethods\GL\glut.h

?????文件??????20932??2005-08-10?10:19??IkMethods\GL\wglext.h

?????文件????8340728??2006-09-19?16:32??IkMethods\glui32.lib

?????文件??????85256??2005-07-28?11:46??IkMethods\glut32.lib

?????文件?????789504??2010-11-07?20:59??IkMethods\IkMethods.ncb

............此處省略33個文件信息