C++ dVector类代码示例

// Takes the scalar multiplicationof a dVector v with a scalar k
dVector RKF45::scalar_multiplication(double k, const dVector& v) const {
  dVector output;
  for (dVector::const_iterator it=v.begin(); it != v.end(); ++it) {
    output.push_back(k * (*it));
  }
  return output;
}

dVector ortogonalizacao(const dVector u, const dVector v)
{
    double escalar = produtoEscalar(u, v) / produtoEscalar(v, v);
    dVector vetor(u.size());
    for(int i = 0;i< u.size();i++)vetor[i]=u[i] - escalar*v[i];
    return vetor;
}

void Toolbox::calculateGlobalMeanAndStd(DataSet &X,dVector& mean,dVector& stdDev)
{
	calculateGlobalMean(X,mean);
	int nbElements = 0;

	//Calculate standard deviation
	stdDev.set(0);
	for(int i = 0;i < (int)X.size() ;i++)
	{
		double* pData = X.at(i)->getPrecomputedFeatures()->get();
		int Width = X.at(i)->getPrecomputedFeatures()->getWidth();
		int Height = X.at(i)->getPrecomputedFeatures()->getHeight();

		for(int col=0; col < Width; col++)
		{
			double* pStdDev = stdDev.get();
			double* pMean = mean.get();
			for(int row = 0; row < Height;row++)
			{
				*pStdDev += (*pData-*pMean) * (*pData-*pMean);
				pStdDev++;
				pData++;
				pMean++;
			}
		}
		nbElements+=Width;
	}
	stdDev.multiply(1.0/(double)nbElements);
	stdDev.eltSqrt();
}

// Adds the dVector b to the dVector a. a is modified in-place.
void RKF45::sum_in_place(dVector& a, const dVector& b) const {
    assert ( a.size() == b.size()
	   && "To sum two vectors, they must be the same size.");
    for (unsigned int i = 0; i < a.size(); i++) {
      a[i] += b[i];
    }
}

// Calculates the 2-norm of the dVector v
double RKF45::norm(const dVector& v) const {
  double output = 0;
  for (dVector::const_iterator it=v.begin(); it != v.end(); ++it) {
    output += (*it) * (*it);
  }
  return sqrt(output);
}

void BLASInterface::Madd(dVector& v,const dVector& x,double a)
{
  integer n=x.n;
  integer vinc = v.stride;
  integer xinc = x.stride;
  daxpy_(&n,&a,x.getStart(),&xinc,v.getStart(),&vinc);
}

double BLASInterface::Dot(const dVector& x,const dVector& y)
{
  Assert(x.n == y.n);
  integer n=x.n;
  integer xinc = x.stride;
  integer yinc = y.stride;
  return ddot_(&n,x.getStart(),&xinc,y.getStart(),&yinc);
}

double GradientDD::computeGradient(dVector& vecGradrient, Model* m,DataSequence*)
{
	dVector tmpVec;
	vecGradrient = *(m->getWeights());
	vecGradrient.add(mu);
	tmpVec = vecGradrient;
	tmpVec.transpose();
	tmpVec.multiply(vecGradrient);
	double f = exp(-0.5*tmpVec[0]);
	vecGradrient.multiply(f);
	return f;
}

void Toolbox::calculateGlobalMean(DataSet &X,dVector& mean)
{
	dVector seqSum;
	int nbElements = 0;

	//Calculate mean
	for(int i = 0;i < (int)X.size() ;i++)
	{
		X.at(i)->getPrecomputedFeatures()->rowSum(seqSum);
		mean.add(seqSum);
		nbElements+=X.at(i)->getPrecomputedFeatures()->getWidth();
	}
	mean.multiply(1.0/(double)nbElements);

}

void dCustomBallAndSocket::SubmitConstraintTwistLimits(const dMatrix& matrix0, const dMatrix& matrix1, const dVector& relOmega, dFloat timestep)
{
	dFloat jointOmega = relOmega.DotProduct3(matrix0.m_front);
	dFloat twistAngle = m_twistAngle.GetAngle() + jointOmega * timestep;
	if (twistAngle < m_minTwistAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_minTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
	} else if (twistAngle > m_maxTwistAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_maxTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);

	} else if (m_twistFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

		dFloat accel = NewtonUserJointCalculateRowZeroAccelaration(m_joint);
		NewtonUserJointSetRowAcceleration(m_joint, accel);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
	}
}

// ----------------------------------------------------------------------
double derivative(int i, const dVector& v, double lattice_spacing) {
  // A local "num points" variable. Used for simplicity.
  int num_points = get_num_points(v);
  assert (0 <= i && i < (int)v.size()
	  && "The ith elemennt must be in the vector");

  if ( i % num_points == 0 ) { // We're at the lower boundary of the grid.
    if (DEBUGGING) {
      cout << "\tUsing forward difference." << endl;
    }
    return forward_difference(i,v,lattice_spacing);
  }
  else if (i % num_points == num_points - 1) {
    // We're at the upper boundary of the grid.
    if (DEBUGGING) {
      cout << "\tUsing backward difference." << endl;
    }
    return backward_difference(i,v,lattice_spacing);
  }
  else { // We're in the middle of the grid.
    if (DEBUGGING) {
      cout << "\tUsing centered difference." << endl;
    }
    return centered_difference(i,v,lattice_spacing);
  }
}

void dComplentaritySolver::dBodyState::IntegrateForce (dFloat timestep, const dVector& force, const dVector& torque)
{
	dVector accel (force.Scale (m_invMass));
	dVector alpha (m_invInertia.RotateVector(torque));
	m_veloc += accel.Scale (timestep);
	m_omega += alpha.Scale (timestep);
}

dVector CustomPlayerController::CalculateDesiredVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, const dVector& gravity, dFloat timestep) const
{
	dMatrix matrix;
	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	dVector updir (matrix.RotateVector(m_upVector));
	dVector frontDir (matrix.RotateVector(m_frontVector));
	dVector rightDir (frontDir * updir);

	dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
	if ((verticalSpeed <= 0.0f) && (m_groundPlane % m_groundPlane) > 0.0f) {
		// plane is supported by a ground plane, apply the player input velocity
		if ((m_groundPlane % updir) >= m_maxSlope) {
			// player is in a legal slope, he is in full control of his movement
			dVector bodyVeloc;
			NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
			veloc = updir.Scale(bodyVeloc % updir) + gravity.Scale (timestep) + frontDir.Scale (forwardSpeed) + rightDir.Scale (lateralSpeed) + updir.Scale(verticalSpeed);
			veloc += (m_groundVelocity - updir.Scale (updir % m_groundVelocity));

			dFloat speedLimitMag2 = forwardSpeed * forwardSpeed + lateralSpeed * lateralSpeed + verticalSpeed * verticalSpeed + m_groundVelocity % m_groundVelocity + 0.1f;
			dFloat speedMag2 = veloc % veloc;
			if (speedMag2 > speedLimitMag2) {
				veloc = veloc.Scale (dSqrt (speedLimitMag2 / speedMag2));
			}

			dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
			if (normalVeloc < 0.0f) {
				veloc -= m_groundPlane.Scale (normalVeloc);
			}
		} else {
			// player is in an illegal ramp, he slides down hill an loses control of his movement 
			NewtonBodyGetVelocity(m_body, &veloc[0]);
			veloc += updir.Scale(verticalSpeed);
			veloc += gravity.Scale (timestep);
			dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
			if (normalVeloc < 0.0f) {
				veloc -= m_groundPlane.Scale (normalVeloc);
			}
		}
	} else {
		// player is on free fall, only apply the gravity
		NewtonBodyGetVelocity(m_body, &veloc[0]);
		veloc += updir.Scale(verticalSpeed);
		veloc += gravity.Scale (timestep);
	}
	return veloc;
}

void Quaternion::set_v(const dVector & v)
{
	if(v.size() == 3) {
		v_ = v;
	}
	else {
		assert (0 && "Quaternion::set_v: input has a wrong size.");
	}
}

void OutputASCIIShade(ostream& out,const dVector& x,double scale)
{
  if(scale == 0) scale = x.maxAbsElement();
  out<<scale<<" x ";
  if(scale == 0) scale = 1;
  out<<'[';
  for(int i=0;i<x.n;i++)
    out<<ASCIIShade(x(i)/scale);
  out<<']';
}