Ejemplo n.º 1
0
dgUnsigned32 dgHingeConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;
	dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));

	m_angle = -angle.m_x;

	dgAssert (dgAbsf (1.0f - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (1.0f - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (1.0f - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir0 = matrix0.m_front;
	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

	const dgVector& p0 = matrix0.m_posit;
	const dgVector& p1 = matrix1.m_posit;
	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

//	dgAssert (((p1 - p0) % (p1 - p0)) < 1.0e-2f);

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);

	CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir1, pointDataQ, &m_jointForce[3]); 
	CalculatePointDerivative (4, params, dir2, pointDataQ, &m_jointForce[4]); 

	dgInt32 ret = 5;
	if (m_jointAccelFnt) {
		dgJointCallbackParam axisParam;
		axisParam.m_accel = dgFloat32 (0.0f);
		axisParam.m_timestep = params.m_timestep;
		axisParam.m_minFriction = DG_MIN_BOUND;
		axisParam.m_maxFriction = DG_MAX_BOUND;

		if (m_jointAccelFnt (*this, &axisParam)) {
			if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[5].m_low = axisParam.m_minFriction;
				params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
				params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculateAngularDerivative (5, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[5]);
//			params.m_jointAccel[5] = axisParam.m_accel;
			SetMotorAcceleration (5, axisParam.m_accel, params);
			ret = 6;
		}
	}

	return dgUnsigned32 (ret);
}
dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgInt32 ret;
	dgFloat32 sinAngle;
	dgFloat32 cosAngle;
	dgMatrix matrix0;
	dgMatrix matrix1;

	CalculateGlobalMatrixAndAngle (matrix0, matrix1);

	const dgVector& dir0 = matrix0.m_front;
	const dgVector& dir1 = matrix1.m_up;
	dgVector dir2 (dir0 * dir1);

	dgVector dir3 (dir2 * dir0);
	dir3 = dir3.Scale3 (dgRsqrt (dir3 % dir3));

	const dgVector& p0 = matrix0.m_posit;
	const dgVector& p1 = matrix1.m_posit;

	dgVector q0 (p0 + dir3.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + dir1.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);

	CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]); 
	ret = 4;


//	dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
//	m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
//	if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
//		m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
//	}

	sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
	cosAngle = matrix0.m_up % matrix1.m_up;
//	dgAssert (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
	m_angle0 = dgAtan2 (sinAngle, cosAngle);

//	dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
//	m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
//	if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
//		m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
//	}

	sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
	cosAngle = matrix0.m_front % matrix1.m_front;
//	dgAssert (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
	m_angle1 = dgAtan2 (sinAngle, cosAngle);

	if (m_jointAccelFnt) {
		dgUnsigned32 code;
		dgJointCallbackParam axisParam[2];

		// linear acceleration
		axisParam[0].m_accel = dgFloat32 (0.0f);
		axisParam[0].m_timestep = params.m_timestep;
		axisParam[0].m_minFriction = DG_MIN_BOUND;
		axisParam[0].m_maxFriction = DG_MAX_BOUND;

		// angular acceleration
		axisParam[1].m_accel = dgFloat32 (0.0f);
		axisParam[1].m_timestep = params.m_timestep;
		axisParam[1].m_minFriction = DG_MIN_BOUND;
		axisParam[1].m_maxFriction = DG_MAX_BOUND;

		code = m_jointAccelFnt (*this, axisParam);
		if (code & 1) {
			if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

//			CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]); 
			CalculateAngularDerivative (ret, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
			//params.m_jointAccel[ret] = axisParam[0].m_accel;
			SetMotorAcceleration (ret, axisParam[0].m_accel, params);
			ret ++;
		}

		if (code & 2) {
			if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}
			CalculateAngularDerivative (ret, params, dir1, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
			//params.m_jointAccel[ret] = axisParam[1].m_accel;
			SetMotorAcceleration (ret, axisParam[1].m_accel, params);
			ret ++;

		}
	}
	return dgUnsigned32 (ret);
}
dgUnsigned32 dgCorkscrewConstraint::JacobianDerivative (dgContraintDescritor& params)
{
	dgMatrix matrix0;
	dgMatrix matrix1;

	dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));

	m_angle = -angle.m_x;
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
	matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);

	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); 

	const dgVector& dir1 = matrix0.m_up;
	const dgVector& dir2 = matrix0.m_right;

//	const dgVector& p0 = matrix0.m_posit;
//	const dgVector& p1 = matrix1.m_posit;
	dgVector p0 (matrix0.m_posit);
	dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));

	dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
	dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));

	dgPointParam pointDataP;
	dgPointParam pointDataQ;
	InitPointParam (pointDataP, m_stiffness, p0, p1);
	InitPointParam (pointDataQ, m_stiffness, q0, q1);

	CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); 
	CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); 
	CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); 
	CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); 

	dgInt32 ret = 4;
	if (m_jointAccelFnt) {
		dgUnsigned32 code;
		dgJointCallbackParam axisParam[2];

		// linear acceleration
		axisParam[0].m_accel = dgFloat32 (0.0f);
		axisParam[0].m_timestep = params.m_timestep;
		axisParam[0].m_minFriction = DG_MIN_BOUND;
		axisParam[0].m_maxFriction = DG_MAX_BOUND;

		// angular acceleration
		axisParam[1].m_accel = dgFloat32 (0.0f);
		axisParam[1].m_timestep = params.m_timestep;
		axisParam[1].m_minFriction = DG_MIN_BOUND;
		axisParam[1].m_maxFriction = DG_MAX_BOUND;

		code = m_jointAccelFnt (*this, axisParam);
		if (code & 1) {
			if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

			CalculatePointDerivative (ret, params, matrix0.m_front, pointDataP, &m_jointForce[ret]); 
			//params.m_jointAccel[ret] = axisParam[0].m_accel;
			SetMotorAcceleration (ret, axisParam[0].m_accel, params);
			ret ++;
		}


		if (code & 2) {
			if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
				params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
				params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
				params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
			}

//			dgVector p (p0 +  dir1);
//			dgPointParam pointData;
//			InitPointParam (pointData, m_stiffness, p, p);
//			CalculatePointDerivative (ret, params, dir2, pointData, &m_jointForce[ret]); 
			CalculateAngularDerivative (ret, params, matrix0.m_front, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
			//params.m_jointAccel[ret] = axisParam[1].m_accel;
			SetMotorAcceleration (ret, axisParam[1].m_accel, params);
			ret ++;
		}
	}

	return dgUnsigned32 (ret);
}