Exemple #1
0
 XXXX ()
 {
    dMatrix s;
    dFloat m = 2.0f;
    for (int i = 0; i < 3; i ++)
    {
       for (int j = 0; j < 3; j ++)
       {
          s[i][j] = m;
          m += (i + 1) + j;
       }
    }
    s.m_posit = dVector (1, 2, 3, 1);
    dMatrix matrix;
    dVector scale;
    dMatrix stretch;
    s.PolarDecomposition (matrix, scale, stretch);
    dMatrix s1 (matrix, scale, stretch);
    dMatrix xxx (dPitchMatrix (30.0f * 3.14159f / 180.0f) * dRollMatrix (30.0f * 3.14159f / 180.0f));
    dMatrix xxxx (GetIdentityMatrix());
    xxx[0] = xxx[0].Scale (-1.0f);
    dFloat mmm = (xxx[0] * xxx[1]) % xxx[2];
    xxxx[0][0] = 3.0f;
    xxxx[1][1] = 3.0f;
    xxxx[2][2] = 4.0f;
    dMatrix xxx2 (xxx * xxxx);
    mmm = (xxx2[0] * xxx2[1]) % xxx2[2];
    xxx2.PolarDecomposition (matrix, scale, stretch);
    s1 = dMatrix (matrix, scale, stretch);
    s1 = dMatrix (matrix, scale, stretch);
 }
dMatrix ARSByComplementaryFilterAndRotationMatrix::filter(const double gyro[3], const double accel[3], const double magn[3], const double &dt)
{
	computeGyroAverage(average_gyro_, gyro, 10000, 10.);

	// 자이로 센서의 바이어스 값을 보정하기 위해 각속도 값에서 각속도 평균값을 뺀다.
	// 그리고 각도의 변화량을 회전행렬로 변환한다.
	dMatrix dR = RotationMatrix(
		_DEG2RAD*(gyro[0] - average_gyro_[0])*dt, 
		_DEG2RAD*(gyro[1] - average_gyro_[1])*dt, 
		_DEG2RAD*(gyro[2] - average_gyro_[2])*dt
	);

	// 회전 행렬의 곱은 두 각도를 더하는 효과를 가진다.
	// R*dR은 R만큼 회전된 좌표계를 dR만큼 더 회전하게 된다.
	R_ = R_ * dR;

	dMatrix g = dMatrix(3, 1, accel);
	dMatrix m = dMatrix(3, 1, magn);

	// accel과 magn은 센서 좌표계를 기준으로 측정된 값이다.
	// 센서의 자세(R)을 곱해서 전역좌표계를 기준으로 한 값으로 바꿔준다.
	g = R_ * g;
	m = R_ * m;

	// 중력가속도 값이 1근처에 있을 때 이득(k1)이 큰 값을 가지도록 한다.
	double l = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]) - 1.;
	double sigma = 0.1;
	double k1 = 0.1 * std::exp(-l * l / (sigma * sigma));

#if defined ADIS_16405
	double k2 = 0.1;

	// 각도의 보정량을 계산한다.
	double dPhi   = g(2,0) ? std::atan(g(1,0) / g(2,0)) : 0.;
	double dTheta = (-1 < g(0,0) && g(0,0) < 1) ? std::asin(-g(0,0) / -1.) : 0.;
	double dPsi   = -std::atan2(m(1,0), m(0,0));
#else
	double k2 = 0.;

	// 각도의 보정량을 계산한다.
	double dPhi   = g(2,0) ? std::atan(g(1,0) / g(2,0)) : 0.;
	double dTheta = (-1 < g(0,0) && g(0,0) < 1) ? std::asin(-g(0,0) / -1.) : 0.;
	double dPsi   = 0.;
#endif

	adjust_rpy_[0] = dPhi;
	adjust_rpy_[1] = dTheta;
	adjust_rpy_[2] = dPsi;

	// 오일러각으로 계산한 보정량을 회전행렬로 변환한다.
	dMatrix aR = RotationMatrix(k1 * dPhi, k1 * dTheta, k2 * dPsi);

	// 계산된 보정량(aR)은 전역좌표계를 기준으로 계산된 값이다.
	// 그러므로 R의 앞에서 aR을 곱해야 한다.
	// R*aR 과 같이 쓰지 않도록 주의한다.
	R_ = aR * R_;

	return R_;
}
Exemple #3
0
dMatrix dMatrix::Transpose () const
{
   return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, 0.0f),
                   dVector (m_front.m_y, m_up.m_y, m_right.m_y, 0.0f),
                   dVector (m_front.m_z, m_up.m_z, m_right.m_z, 0.0f),
                   dVector (0.0f, 0.0f, 0.0f, 1.0f));
}
Exemple #4
0
dMatrix dGetZeroMatrix ()
{
   return dMatrix (dVector (0.0f, 0.0f, 0.0f, 0.0f),
                   dVector (0.0f, 0.0f, 0.0f, 0.0f),
                   dVector (0.0f, 0.0f, 0.0f, 0.0f),
                   dVector (0.0f, 0.0f, 0.0f, 0.0f));
}
Exemple #5
0
dMatrix dMatrix::Inverse () const
{
   return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, 0.0f),
                   dVector (m_front.m_y, m_up.m_y, m_right.m_y, 0.0f),
                   dVector (m_front.m_z, m_up.m_z, m_right.m_z, 0.0f),
                   dVector (- (m_posit % m_front), - (m_posit % m_up), - (m_posit % m_right), 1.0f));
}
Exemple #6
0
dMatrix dGetIdentityMatrix()
{
   return dMatrix (dVector (1.0f, 0.0f, 0.0f, 0.0f),
                   dVector (0.0f, 1.0f, 0.0f, 0.0f),
                   dVector (0.0f, 0.0f, 1.0f, 0.0f),
                   dVector (0.0f, 0.0f, 0.0f, 1.0f));
}
Exemple #7
0
      // Returns a reference to a satTypeValueMap object after converting
      // from a geocentric reference system to a topocentric reference system.
      //
      // @param gData     Data object holding the data.
      //
   satTypeValueMap& XYZ2NED::Process(satTypeValueMap& gData)
      throw(ProcessingException)
   {

      try
      {

         Matrix<double> neuMatrix;

            // Get the corresponding geometry/design matrix data
         Matrix<double> dMatrix(gData.getMatrixOfTypes(inputSet));

            // Compute the base change. For convenience, we use the property:
            // Y = A*B => Y^T = (A*B)^T => Y^T = B^T * A^T
         neuMatrix = dMatrix*rotationMatrix;

         gData.insertMatrix(outputSet, neuMatrix);

         return gData;

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

   }  // End of method 'XYZ2NED::Process()'
Exemple #8
0
void dMeshNodeInfo::BakeTransform (const dMatrix& transform)
{
	dVector scale; 
	dMatrix stretchMatrix;

//	dMatrix matrix (m_matrix * transform);
//	matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
//	matrix = dMatrix (GetIdentityMatrix(), scale, stretchMatrix);

	dMatrix tmp (m_matrix);
	dMatrix matrix (transform.Inverse4x4() * m_matrix * transform);
	matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
	matrix = transform * dMatrix (GetIdentityMatrix(), scale, stretchMatrix);

	int pointCount = NewtonMeshGetPointCount (m_mesh); 
	int pointStride = NewtonMeshGetPointStrideInByte (m_mesh) / sizeof (dFloat);
	dFloat* const points = NewtonMeshGetPointArray (m_mesh); 
	matrix.TransformTriplex(points, pointStride * sizeof (dFloat), points, pointStride * sizeof (dFloat), pointCount);


	dFloat* const normals = NewtonMeshGetNormalArray(m_mesh); 
	dMatrix rotation (matrix.Inverse4x4().Transpose() * matrix);
	rotation.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	rotation.TransformTriplex(normals, pointStride * sizeof (dFloat), normals, pointStride * sizeof (dFloat), pointCount);

	int vertexCount = NewtonMeshGetVertexCount (m_mesh); 
	int vertexStride = NewtonMeshGetVertexStrideInByte (m_mesh) / sizeof (dFloat);
	dFloat* const vertex = NewtonMeshGetVertexArray (m_mesh); 
	matrix.TransformTriplex(vertex, vertexStride * sizeof (dFloat), vertex, vertexStride * sizeof (dFloat), vertexCount);
}
Exemple #9
0
dMatrix dMatrix::Transpose4X4 () const
{
   return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x),
                   dVector (m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y),
                   dVector (m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z),
                   dVector (m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w));
}
Exemple #10
0
vector<vector<double> > LinearAlgebra::getObservedEuclideanDistance(vector<vector<double> >& relAbundData){

	int numSamples = relAbundData.size();
	int numOTUs = relAbundData[0].size();
	
	vector<vector<double> > dMatrix(numSamples);
	for(int i=0;i<numSamples;i++){
		dMatrix[i].resize(numSamples);
	}
	
	for(int i=0;i<numSamples;i++){
		for(int j=0;j<numSamples;j++){
			
			double d = 0;
			for(int k=0;k<numOTUs;k++){
				d += pow((relAbundData[i][k] - relAbundData[j][k]), 2.0000);
			}
			dMatrix[i][j] = pow(d, 0.50000);
			dMatrix[j][i] = dMatrix[i][j];
			
		}
	}
	return dMatrix;
	
}
	void SubmitConstraints(dFloat timestep, int threadIndex)
	{
		CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
		float invTimestep = 1.0f / timestep;

		dMatrix matrix0;
		dMatrix matrix1;

		CalculateGlobalMatrix(matrix0, matrix1);

		if (m_anim_speed != 0.0f) // some animation to illustrate purpose
		{
			m_anim_time += timestep * m_anim_speed;
			float a0 = sin(m_anim_time);
			float a1 = m_anim_offset * 3.14f;
			dVector axis(sin(a1), 0.0f, cos(a1));
			//dVector axis (1,0,0);
			m_target = dQuaternion(axis, a0 * 0.5f);
		}

		// measure error
		dQuaternion q0(matrix0);
		dQuaternion q1(matrix1);
		dQuaternion qt0 = m_target * q1;
		dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f)	? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;

		float errorAngle = 2.0f * acos(dMax(-1.0f, dMin(1.0f, qErr.m_q0)));
		dVector errorAngVel(0, 0, 0);

		dMatrix basis;
		if (errorAngle > 1.0e-10f) {
			dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
			errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis % errorAxis));
			errorAngVel = errorAxis.Scale(errorAngle * invTimestep);

			basis = dGrammSchmidt(errorAxis);
		} else {
			basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
		}

		dVector angVel0, angVel1;
		NewtonBodyGetOmega(m_body0, (float*)&angVel0);
		NewtonBodyGetOmega(m_body1, (float*)&angVel1);

		dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);

		// motor
		for (int n = 0; n < 3; n++) {
			// calculate the desired acceleration
			dVector &axis = basis[n];
			float relAccel = angAcc % axis;

			NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
			NewtonUserJointSetRowAcceleration(m_joint, relAccel);
			NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		}
	}
Exemple #12
0
dMatrix dRollMatrix (dFloat ang)
{
   dFloat cosAng;
   dFloat sinAng;
   sinAng = dSin (ang);
   cosAng = dCos (ang);
   return dMatrix (dVector ( cosAng, sinAng, 0.0f, 0.0f),
                   dVector (-sinAng, cosAng, 0.0f, 0.0f),
                   dVector (   0.0f,   0.0f, 1.0f, 0.0f),
                   dVector (   0.0f,   0.0f, 0.0f, 1.0f));
}
void dLineNodeInfo::BakeTransform (const dMatrix& transform)
{
	dVector scale; 
	dMatrix stretchMatrix;

	dMatrix matrix (transform.Inverse4x4() * m_matrix * transform);
	matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
	matrix = transform * dMatrix (dGetIdentityMatrix(), scale, stretchMatrix);

	matrix.TransformTriplex (&m_curve.GetControlPointArray()[0].m_x, sizeof (dVector), &m_curve.GetControlPointArray()[0].m_x, sizeof (dVector), m_curve.GetControlPointCount());
}
void dMeshNodeInfo::BakeTransform (const dMatrix& transform)
{
	dVector scale; 
	dMatrix stretchMatrix;

	//dMatrix tmp (m_matrix);
	dMatrix matrix (transform.Inverse4x4() * m_matrix * transform);
	matrix.PolarDecomposition (m_matrix, scale, stretchMatrix);
	matrix = transform * dMatrix (dGetIdentityMatrix(), scale, stretchMatrix);

	NewtonMeshApplyTransform (m_mesh, &matrix[0][0]);
}
Exemple #15
0
dMatrix dGrammSchmidt (const dVector & dir)
{
   dVector up;
   dVector right;
   dVector front (dir);
   front = front.Scale (1.0f / dSqrt (front % front));
   if (dAbs (front.m_z) > 0.577f)
      right = front * dVector (-front.m_y, front.m_z, 0.0f);
   else
      right = front * dVector (-front.m_y, front.m_x, 0.0f);
   right = right.Scale (1.0f / dSqrt (right % right));
   up = right * front;
   front.m_w = 0.0f;
   up.m_w = 0.0f;
   right.m_w = 0.0f;
   return dMatrix (front, up, right, dVector (0.0f, 0.0f, 0.0f, 1.0f));
}
void DemoEntity::InterpolateMatrix (DemoEntityManager& world, dFloat param)
{
	// read the data in a critical section to prevent race condition from oteh thread  
	world.Lock(m_lock);

	dVector p0(m_curPosition);
	dVector p1(m_nextPosition);
	dQuaternion r0 (m_curRotation);
	dQuaternion r1 (m_nextRotation);

	// release the critical section
	world.Unlock(m_lock);

	dVector posit (p0 + (p1 - p0).Scale (param));
	dQuaternion rotation (r0.Slerp(r1, param));

	m_matrix = dMatrix (rotation, posit);
}
Exemple #17
0
dMatrix dMatrix::operator* (const dMatrix & B) const
{
   const dMatrix & A = *this;
   return dMatrix (dVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0],
                            A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
                            A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
                            A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
                   dVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0],
                            A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
                            A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
                            A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
                   dVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0],
                            A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
                            A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
                            A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
                   dVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0],
                            A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1],
                            A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2],
                            A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3]));
}
void dComplentaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
{
	const dFloat D_MAX_ANGLE_STEP = dFloat (45.0f * 3.141592f / 180.0f);
	const dFloat D_ANGULAR_TOL = dFloat (0.0125f * 3.141592f / 180.0f);

	m_globalCentreOfMass += m_veloc.Scale (timestep); 
	while (((m_omega % m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
		m_omega = m_omega.Scale (dFloat (0.8f));
	}

	// this is correct
	dFloat omegaMag2 = m_omega % m_omega;
	if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
		dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
		dVector omegaAxis (m_omega.Scale (invOmegaMag));
		dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
		dQuaternion rotation (omegaAxis, omegaAngle);
		dQuaternion rotMatrix (m_matrix);
		rotMatrix = rotMatrix * rotation;
		rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
		m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
	}

	m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);

#ifdef _DEBUG
	int j0 = 1;
	int j1 = 2;
	for (int i = 0; i < 3; i ++) {
		dAssert (m_matrix[i][3] == 0.0f);
		dFloat val = m_matrix[i] % m_matrix[i];
		dAssert (dAbs (val - 1.0f) < 1.0e-5f);
		dVector tmp (m_matrix[j0] * m_matrix[j1]);
		val = tmp % m_matrix[i];
		dAssert (dAbs (val - 1.0f) < 1.0e-5f);
		j0 = j1;
		j1 = i;
	}
#endif
}
dMatrix DemoEntity::GetNextMatrix () const
{
	return dMatrix (m_nextRotation, m_nextPosition);
}
dNewtonHingeJoint::dNewtonHingeJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
	:dNewtonJoint(m_hinge)
{
	SetJoint (new dCustomHinge (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
dNewtonBallAndSocketJoint::dNewtonBallAndSocketJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_ballAndSocket)
{
    SetJoint (new dCustomBallAndSocket (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
dNewtonCylindricalJoint::dNewtonCylindricalJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
    :dNewtonJoint(m_cylindrical)
{
    SetJoint (new dCustomCorkScrew (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
dNewtonDoubleHinge::dNewtonDoubleHinge(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
	:dNewtonJoint(m_universal)
{
	SetJoint (new dCustomDoubleHinge (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
dNewtonSliderJoint::dNewtonSliderJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1)
	:dNewtonJoint(m_slider)
{
	SetJoint (new dCustomSlider (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL));
}
void* dNewtonArticulationManager::dNewtonArticulationController::AddBone (dNewtonBody* const boneBody, const dFloat* const bindMatrix, void* const parentBone)
{
	CustomArticulatedTransformController::dSkeletonBone* const parent = (CustomArticulatedTransformController::dSkeletonBone*) parentBone;
	CustomArticulatedTransformController::dSkeletonBone* const bone = m_controller->AddBone (boneBody->GetNewtonBody(), dMatrix (bindMatrix), parent);
	if (parent) {
		dNewtonBody* const parentBody = (dNewtonBody*) NewtonBodyGetUserData(parent->m_body);
		parentBody->AttachChild (boneBody);
	}

	// save the bone articulation in the body
	boneBody->SetBoneArticulation(bone);

	return bone;
}
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex)
{
	dMatrix matrix; 
	dQuaternion bodyRotation;
	dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); 
	dVector omega(0.0f, 0.0f, 0.0f, 0.0f);  

	CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
	NewtonWorld* const world = manager->GetWorld();

	// apply the player motion, by calculation the desired plane linear and angular velocity
	manager->ApplyPlayerMove (this, timestep);

	// get the body motion state 
	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	NewtonBodyGetVelocity(m_body, &veloc[0]);
	NewtonBodyGetOmega(m_body, &omega[0]);

	// integrate body angular velocity
	NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); 
	bodyRotation = bodyRotation.IntegrateOmega(omega, timestep);
	matrix = dMatrix (bodyRotation, matrix.m_posit);

	// integrate linear velocity
	dFloat normalizedTimeLeft = 1.0f; 
	dFloat step = timestep * dSqrt (veloc % veloc) ;
	dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS);
	int prevContactCount = 0;
	CustomControllerConvexCastPreFilter castFilterData (m_body);
	NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS];

	dVector updir (matrix.RotateVector(m_upVector));

	dVector scale;
	NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z);
	//const dFloat radio = m_outerRadio * 4.0f;
	const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f;
	NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio);


	NewtonWorldConvexCastReturnInfo upConstratint;
	memset (&upConstratint, 0, sizeof (upConstratint));
	upConstratint.m_normal[0] = m_upVector.m_x;
	upConstratint.m_normal[1] = m_upVector.m_y;
	upConstratint.m_normal[2] = m_upVector.m_z;
	upConstratint.m_normal[3] = m_upVector.m_w;

	for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) {
		if ((veloc % veloc) < 1.0e-6f) {
			break;
		}

		dFloat timetoImpact;
		NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS];
		dVector destPosit (matrix.m_posit + veloc.Scale (timestep));
		int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
		if (contactCount) {
			contactCount = manager->ProcessContacts (this, info, contactCount);
		}

		if (contactCount) {
			matrix.m_posit += veloc.Scale (timetoImpact * timestep);
			if (timetoImpact > 0.0f) {
				matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; 
			}

			normalizedTimeLeft -= timetoImpact;

			dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
			dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
			dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2];

			for (int i = 1; i < contactCount; i ++) {
				dVector n0 (info[i-1].m_normal);
				for (int j = 0; j < i; j ++) {
					dVector n1 (info[j].m_normal);
					if ((n0 % n1) > 0.9999f) {
						info[i] = info[contactCount - 1];
						i --;
						contactCount --;
						break;
					}
				}
			}

			int count = 0;
			if (!m_isJumping) {
				upConstratint.m_point[0] = matrix.m_posit.m_x;
				upConstratint.m_point[1] = matrix.m_posit.m_y;
				upConstratint.m_point[2] = matrix.m_posit.m_z;
				upConstratint.m_point[3] = matrix.m_posit.m_w;

				speed[count] = 0.0f;
				bounceNormal[count] = dVector (upConstratint.m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint);
				count ++;
			}

			for (int i = 0; i < contactCount; i ++) {
				speed[count] = 0.0f;
				bounceNormal[count] = dVector (info[i].m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]);
				count ++;
			}

			for (int i = 0; i < prevContactCount; i ++) {
				speed[count] = 0.0f;
				bounceNormal[count] = dVector (prevInfo[i].m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]);
				count ++;
			}

			dFloat residual = 10.0f;
			dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);
			for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) {
				residual = 0.0f;
				for (int k = 0; k < count; k ++) {
					dVector normal (bounceNormal[k]);
					dFloat v = bounceSpeed[k] - normal % auxBounceVeloc;
					dFloat x = speed[k] + v;
					if (x < 0.0f) {
						v = 0.0f;
						x = 0.0f;
					}

					if (dAbs (v) > residual) {
						residual = dAbs (v);
					}

					auxBounceVeloc += normal.Scale (x - speed[k]);
					speed[k] = x;
				}
			}

			dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f);
			for (int i = 0; i < count; i ++) {
				dVector normal (bounceNormal[i]);
				velocStep += normal.Scale (speed[i]);
			}
			veloc += velocStep;

			dFloat velocMag2 = velocStep % velocStep;
			if (velocMag2 < 1.0e-6f) {
				dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep);
				matrix.m_posit += veloc.Scale (advanceTime);
				normalizedTimeLeft -= advanceTime / timestep;
			}

			prevContactCount = contactCount;
			memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));

		} else {
			matrix.m_posit = destPosit;
			matrix.m_posit.m_w = 1.0f;
			break;
		}
	}
	NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z);

	// determine if player is standing on some plane
	dMatrix supportMatrix (matrix);
	supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin);
	if (m_isJumping) {
		dVector dst (matrix.m_posit);
		UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
	} else {
		step = dAbs (updir % veloc.Scale (timestep));
		dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step;
		dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f));
		UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
	}

	// set player velocity, position and orientation
	NewtonBodySetVelocity(m_body, &veloc[0]);
	NewtonBodySetMatrix (m_body, &matrix[0][0]);
}
Exemple #27
0
dMatrix CustomPickBody::GetTargetMatrix () const
{
	return dMatrix (m_targetRot, m_targetPosit);
}
void dagPoseInfo::printDagPoseInfo(MObject& dagPoseNode, unsigned index)
//
// Description:
//   Given a dagPose and an index corresponding to a joint, print out
//   the matrix info for the joint.
// Return:
//	 None.
//
{
    MFnDependencyNode nDagPose(dagPoseNode);
    fprintf(file,"%s\n",nDagPose.name().asChar());

    // construct plugs for this joints world and local matrices
    //
    MObject aWorldMatrix = nDagPose.attribute("worldMatrix");
    MPlug pWorldMatrix(dagPoseNode,aWorldMatrix);
    pWorldMatrix.selectAncestorLogicalIndex(index,aWorldMatrix);

    MObject aMatrix = nDagPose.attribute("xformMatrix");
    MPlug pMatrix(dagPoseNode,aMatrix);
    pMatrix.selectAncestorLogicalIndex(index,aMatrix);

    // get and print the world matrix data
    //
    MObject worldMatrix, xformMatrix;
    MStatus status = pWorldMatrix.getValue(worldMatrix);
    if (MS::kSuccess != status) {
        displayError("Problem retrieving world matrix.");
    } else {
        bool foundMatrix = 0;
        MFnMatrixData dMatrix(worldMatrix);
        MMatrix wMatrix = dMatrix.matrix(&status);
        if (MS::kSuccess == status) {
            foundMatrix = 1;
            unsigned jj,kk;
            fprintf(file,"worldMatrix\n");
            for (jj = 0; jj < 4; ++jj) {
                for (kk = 0; kk < 4; ++kk) {
                    double val = wMatrix(jj,kk);
                    fprintf(file,"%f ",val);
                }
                fprintf(file,"\n");
            }
        }
        if (!foundMatrix) {
            displayError("Error getting world matrix data.");
        }
    }

    // get and print the local matrix data
    //
    status = pMatrix.getValue(xformMatrix);
    if (MS::kSuccess != status) {
        displayError("Problem retrieving xform matrix.");
    } else {
        bool foundMatrix = 0;
        MFnMatrixData dMatrix(xformMatrix);
        if (dMatrix.isTransformation()) {
            MTransformationMatrix xform = dMatrix.transformation(&status);
            if (MS::kSuccess == status) {
                foundMatrix = 1;
                MMatrix xformAsMatrix = xform.asMatrix();
                unsigned jj,kk;
                fprintf(file,"matrix\n");
                for (jj = 0; jj < 4; ++jj) {
                    for (kk = 0; kk < 4; ++kk) {
                        double val = xformAsMatrix(jj,kk);
                        fprintf(file,"%f ",val);
                    }
                    fprintf(file,"\n");
                }
            }
        }
        if (!foundMatrix) {
            displayError("Error getting local matrix data.");
        }
    }
}
dMatrix CustomKinematicController::GetTargetMatrix () const
{
	return dMatrix (m_targetRot, m_targetPosit);
}
void RenderJointsDebugInfo (NewtonWorld* const world, dFloat size)
{
	glDisable(GL_TEXTURE_2D);
	glDisable (GL_LIGHTING);
	glBegin(GL_LINES);

	// this will go over the joint list twice, 
	for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {	
		for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) {
			NewtonJointRecord info;
			NewtonJointGetInfo (joint, &info);

			if (strcmp (info.m_descriptionType, "customJointNotInfo")) {

				// draw first frame
				dMatrix matrix0;
				NewtonBodyGetMatrix (info.m_attachBody_0, &matrix0[0][0]);
				matrix0 = dMatrix (&info.m_attachmenMatrix_0[0][0]) * matrix0;
				dVector o0 (matrix0.m_posit);

				dVector x (o0 + matrix0.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f)));
				glColor3f (1.0f, 0.0f, 0.0f);
				glVertex3f (o0.m_x, o0.m_y, o0.m_z);
				glVertex3f (x.m_x, x.m_y, x.m_z);

				dVector y (o0 + matrix0.RotateVector (dVector (0.0f, size, 0.0f, 0.0f)));
				glColor3f (0.0f, 1.0f, 0.0f);
				glVertex3f (o0.m_x, o0.m_y, o0.m_z);
				glVertex3f (y.m_x, y.m_y, y.m_z);

				dVector z (o0 + matrix0.RotateVector (dVector (0.0f, 0.0f, size, 0.0f)));
				glColor3f (0.0f, 0.0f, 1.0f);
				glVertex3f (o0.m_x, o0.m_y, o0.m_z);
				glVertex3f (z.m_x, z.m_y, z.m_z);


				// draw second frame
				dMatrix matrix1 (dGetIdentityMatrix());
				if (info.m_attachBody_1) {
					NewtonBodyGetMatrix (info.m_attachBody_1, &matrix1[0][0]);
				}
				matrix1 = dMatrix (&info.m_attachmenMatrix_1[0][0]) * matrix1;
				dVector o1 (matrix1.m_posit);

				x = o1 + matrix1.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f));
				glColor3f (1.0f, 0.0f, 0.0f);
				glVertex3f (o1.m_x, o1.m_y, o1.m_z);
				glVertex3f (x.m_x, x.m_y, x.m_z);

				y = o1 + matrix1.RotateVector (dVector (0.0f, size, 0.0f, 0.0f));
				glColor3f (0.0f, 1.0f, 0.0f);
				glVertex3f (o1.m_x, o1.m_y, o1.m_z);
				glVertex3f (y.m_x, y.m_y, y.m_z);

				z = o1 + matrix1.RotateVector (dVector (0.0f, 0.0f, size, 0.0f));
				glColor3f (0.0f, 0.0f, 1.0f);
				glVertex3f (o1.m_x, o1.m_y, o1.m_z);
				glVertex3f (z.m_x, z.m_y, z.m_z);

				if (!strcmp (info.m_descriptionType, "limitballsocket")) {
					// draw the cone limit of this joint
					int steps = 12;
					dMatrix coneMatrix (dRollMatrix(info.m_maxAngularDof[1]));
					dMatrix ratationStep (dPitchMatrix(2.0f * 3.14151693f / steps));

					dVector p0 (coneMatrix.RotateVector(dVector (size * 0.5f, 0.0f, 0.0f, 0.0f)));
					dVector q0 (matrix1.TransformVector(p0));

					glColor3f (1.0f, 1.0f, 0.0f);
					for (int i = 0; i < (steps + 1); i ++) {
						dVector p1 (ratationStep.RotateVector(p0));
						dVector q1 (matrix1.TransformVector(p1));

						glVertex3f (o0.m_x, o0.m_y, o0.m_z);
						glVertex3f (q0.m_x, q0.m_y, q0.m_z);
				
						glVertex3f (q0.m_x, q0.m_y, q0.m_z);
						glVertex3f (q1.m_x, q1.m_y, q1.m_z);

						p0 = p1;
						q0 = q1;
					}
				}
			}
		}
	}
	glEnd();
}