Mat Camera::computePMatrix() {
Mat AR = computeCalibrationMatrix() * computeRotationMatrix();
printMat("A", computeCalibrationMatrix());
printMat("R", computeRotationMatrix());
Mat ARC = AR * computeCameraCenterMatrix(false);
Mat p;
hconcat(AR, ARC.mul(-1), p);
printMat("P", p);
return p;
}
void
GolemMaterialTH::computeProperties()
{
if (_current_elem->dim() < _mesh.dimension())
computeRotationMatrix();
for (_qp = 0; _qp < _qrule->n_points(); ++_qp)
computeQpProperties();
}
Mat Camera::computeDMatrix() {
Mat r = computeRotationMatrix();
double * d_data = new double[16] {
r.at<double>(0,0), r.at<double>(0,1), r.at<double>(0,2), this->cameraCenter[0],
r.at<double>(1,0), r.at<double>(1,1), r.at<double>(1,2), this->cameraCenter[1],
r.at<double>(2,0), r.at<double>(2,1), r.at<double>(2,2), this->cameraCenter[2],
0, 0, 0, 1};
Mat temp = Mat(4,4, CV_64F, d_data);
return temp;
}
void SurfaceObj::rotateC(Vec3f axis, float angle)
{
Mat3x3f rot;
computeRotationMatrix(axis, angle, rot);
for(int i=0;i<Point.size();i++)
Point[i]=rot*Point[i];
computeFaceNormal();
computeCenterPoint();
};
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
double Nx = _Nc->getValue(0);
double Ny = _Nc->getValue(1);
double Nz = _Nc->getValue(2);
double Px = _Pc1->getValue(0);
double Py = _Pc1->getValue(1);
double Pz = _Pc1->getValue(2);
double G1x = q1->getValue(0);
double G1y = q1->getValue(1);
double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
printf("contact normal:\n");
_Nc->display();
printf("point de contact :\n");
_Pc1->display();
printf("center of masse :\n");
q1->display();
#endif
_RotationAbsToContactFrame->setValue(0, 0, Nx);
_RotationAbsToContactFrame->setValue(0, 1, Ny);
_RotationAbsToContactFrame->setValue(0, 2, Nz);
_NPG1->zero();
(*_NPG1)(0, 0) = 0;
(*_NPG1)(0, 1) = -(G1z - Pz);
(*_NPG1)(0, 2) = (G1y - Py);
(*_NPG1)(1, 0) = (G1z - Pz);
(*_NPG1)(1, 1) = 0;
(*_NPG1)(1, 2) = -(G1x - Px);
(*_NPG1)(2, 0) = -(G1y - Py);
(*_NPG1)(2, 1) = (G1x - Px);
(*_NPG1)(2, 2) = 0;
computeRotationMatrix(q1,_rotationMatrixAbsToBody);
prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);
prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));
#ifdef NEFC3D_DEBUG
printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
_jachqT->display();
#endif
}
inline void CVertexRingElement::_compute1RingForceDynamic(
const int needCompRotation,
const Vector3d ringnodepos[], //1ring nodes' current positions in local coordiante
Vector3d F[], //force array, in world coordinate
double3x3 *stiffi[]) //jacobian array
{
//need to copmute the rotation first, there are several methods
if (needCompRotation){
m_R0 = m_R;
const bool r = computeRotationMatrix(ringnodepos, m_R);
if (!r) m_R = m_R0;
}
for (int i=0; i<m_nv; i++){
CVertexRingNode& node = m_pVertexRingNode[i];
CMeMaterialProperty &mtl = *(node.m_pMaterial);
node.computeNodalForce(ringnodepos[i], m_R, mtl, F[i], stiffi[i]);
}
}
RainData::RainData( fpreal now,
long n, UT_Vector3 bmin, UT_Vector3 bmax, UT_Vector3 dir,
fpreal rndMin, fpreal rndMax, int seed, fpreal speed,
fpreal speedVarience)
{
now_ = now;
pointsNumber_ = n;
minimumBounds_ = bmin;
maximumBounds_ = bmax;
rainDirection_ = dir;
directionMatrix_ = computeRotationMatrix(rainDirection_);
rndMin_ = rndMin;
rndMax_ = rndMax;
seed_ = seed;
constantSpeed_ = speed;
speedVarience_ = speedVarience;
noise_.initNoise();
Imath::Rand32 tmpGenerator( seed_ );
rndGenerator_ = tmpGenerator;
//printf("RainData constructor\n");
}
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
double Nx = _Nc->getValue(0);
double Ny = _Nc->getValue(1);
double Nz = _Nc->getValue(2);
double Px = _Pc1->getValue(0);
double Py = _Pc1->getValue(1);
double Pz = _Pc1->getValue(2);
double G1x = q1->getValue(0);
double G1y = q1->getValue(1);
double G1z = q1->getValue(2);
_RotationAbsToContactFrame->setValue(0, 0, Nx);
_RotationAbsToContactFrame->setValue(0, 1, Ny);
_RotationAbsToContactFrame->setValue(0, 2, Nz);
_NPG1->zero();
(*_NPG1)(0, 0) = 0;
(*_NPG1)(0, 1) = -(G1z - Pz);
(*_NPG1)(0, 2) = (G1y - Py);
(*_NPG1)(1, 0) = (G1z - Pz);
(*_NPG1)(1, 1) = 0;
(*_NPG1)(1, 2) = -(G1x - Px);
(*_NPG1)(2, 0) = -(G1y - Py);
(*_NPG1)(2, 1) = (G1x - Px);
(*_NPG1)(2, 2) = 0;
computeRotationMatrix(q1,_rotationMatrixAbsToBody);
prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);
prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));
double G2x = q2->getValue(0);
double G2y = q2->getValue(1);
double G2z = q2->getValue(2);
_NPG2->zero();
(*_NPG2)(0, 0) = 0;
(*_NPG2)(0, 1) = -(G2z - Pz);
(*_NPG2)(0, 2) = (G2y - Py);
(*_NPG2)(1, 0) = (G2z - Pz);
(*_NPG2)(1, 1) = 0;
(*_NPG2)(1, 2) = -(G2x - Px);
(*_NPG2)(2, 0) = -(G2y - Py);
(*_NPG2)(2, 1) = (G2x - Px);
(*_NPG2)(2, 2) = 0;
computeRotationMatrix(q2,_rotationMatrixAbsToBody);
prod(*_NPG2, *_rotationMatrixAbsToBody, *_AUX1, true);
prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);
for (unsigned int jj = 0; jj < 3; jj++)
_jachqT->setValue(0, jj + 6, -_RotationAbsToContactFrame->getValue(0, jj));
for (unsigned int jj = 3; jj < 6; jj++)
_jachqT->setValue(0, jj + 6, -_AUX2->getValue(0, jj - 3));
}
Mat Camera::computeAR() {
Mat temp = computeCalibrationMatrix() * computeRotationMatrix();
return temp;
}
