ResultAlign2D get_complete_alignment_no_preprocessing(
const cv::Mat &input, const cv::Mat &INPUT, const cv::Mat &POLAR1,
cv::Mat &m_to_align, const cv::Mat &POLAR2, bool apply) {
IMP_LOG_TERSE("starting complete 2D alignment with no preprocessing"
<< std::endl);
cv::Mat aux1, aux2, aux3, aux4; // auxiliary matrices
cv::Mat AUX1, AUX2, AUX3; // ffts
algebra::Transformation2D transformation1, transformation2;
double angle1 = 0, angle2 = 0;
ResultAlign2D RA = get_rotational_alignment_no_preprocessing(POLAR1, POLAR2);
angle1 = RA.first.get_rotation().get_angle();
get_transformed(m_to_align, aux1, RA.first); // rotate
get_fft_using_optimal_size(aux1, AUX1);
RA = get_translational_alignment_no_preprocessing(INPUT, AUX1);
algebra::Vector2D shift1 = RA.first.get_translation();
transformation1.set_rotation(angle1);
transformation1.set_translation(shift1);
get_transformed(m_to_align, aux2, transformation1); // rotate
double ccc1 = get_cross_correlation_coefficient(input, aux2);
// Check the opposed angle
if (angle1 < PI) {
angle2 = angle1 + PI;
} else {
angle2 = angle1 - PI;
}
algebra::Rotation2D R2(angle2);
algebra::Transformation2D tr(R2);
get_transformed(m_to_align, aux3, tr); // rotate
get_fft_using_optimal_size(aux3, AUX3);
RA = get_translational_alignment_no_preprocessing(INPUT, AUX3);
algebra::Vector2D shift2 = RA.first.get_translation();
transformation2.set_rotation(angle2);
transformation2.set_translation(shift2);
get_transformed(m_to_align, aux3, transformation2);
double ccc2 = get_cross_correlation_coefficient(input, aux3);
if (ccc2 > ccc1) {
if (apply) {
aux3.copyTo(m_to_align);
}
IMP_LOG_VERBOSE(" Align2D complete Transformation= "
<< transformation2 << " cross_correlation = " << ccc2
<< std::endl);
return ResultAlign2D(transformation2, ccc2);
} else {
if (apply) {
aux3.copyTo(m_to_align);
}
IMP_LOG_VERBOSE(" Align2D complete Transformation= "
<< transformation1 << " cross_correlation = " << ccc1
<< std::endl);
return ResultAlign2D(transformation1, ccc1);
}
}
IMPEM2D_BEGIN_NAMESPACE
ResultAlign2D get_complete_alignment(const cv::Mat &input, cv::Mat &m_to_align,
bool apply) {
IMP_LOG_TERSE("starting complete 2D alignment " << std::endl);
cv::Mat autoc1, autoc2, aux1, aux2, aux3;
algebra::Transformation2D transformation1, transformation2;
ResultAlign2D RA;
get_autocorrelation2d(input, autoc1);
get_autocorrelation2d(m_to_align, autoc2);
RA = get_rotational_alignment(autoc1, autoc2, false);
double angle1 = RA.first.get_rotation().get_angle();
get_transformed(m_to_align, aux1, RA.first); // rotate
RA = get_translational_alignment(input, aux1);
algebra::Vector2D shift1 = RA.first.get_translation();
transformation1.set_rotation(angle1);
transformation1.set_translation(shift1);
get_transformed(m_to_align, aux2, transformation1);
double ccc1 = get_cross_correlation_coefficient(input, aux2);
// Check for both angles that can be the solution
double angle2;
if (angle1 < PI) {
angle2 = angle1 + PI;
} else {
angle2 = angle1 - PI;
}
// rotate
algebra::Rotation2D R2(angle2);
algebra::Transformation2D tr(R2);
get_transformed(m_to_align, aux3, tr);
RA = get_translational_alignment(input, aux3);
algebra::Vector2D shift2 = RA.first.get_translation();
transformation2.set_rotation(angle2);
transformation2.set_translation(shift2);
get_transformed(m_to_align, aux3, transformation2);
double ccc2 = get_cross_correlation_coefficient(input, aux3);
if (ccc2 > ccc1) {
if (apply) {
aux3.copyTo(m_to_align);
}
IMP_LOG_VERBOSE(" Transformation= " << transformation2
<< " cross_correlation = " << ccc2
<< std::endl);
return em2d::ResultAlign2D(transformation2, ccc2);
} else {
if (apply) {
aux2.copyTo(m_to_align);
}
IMP_LOG_VERBOSE(" Transformation= " << transformation1
<< " cross_correlation = " << ccc1
<< std::endl);
return em2d::ResultAlign2D(transformation1, ccc1);
}
}
void Molecular_system::calcLocWithTestPos(Sample_point * sample,
Array1 <doublevar> & tpos,
Array1 <doublevar> & Vtest) {
int nions=sample->ionSize();
int nelectrons=sample->electronSize();
Vtest.Resize(nelectrons + 1);
Vtest = 0;
Array1 <doublevar> oldpos(3);
//cout << "Calculating local energy\n";
sample->getElectronPos(0, oldpos);
sample->setElectronPosNoNotify(0, tpos);
Array1 <doublevar> R(5);
sample->updateEIDist();
sample->updateEEDist();
for(int i=0; i < nions; i++) {
sample->getEIDist(0, i, R);
Vtest(nelectrons)+=-sample->getIonCharge(i)/R(0);
}
doublevar dist = 0.0;
for(int d=0; d<3; d++) {
dist += (oldpos(d) - tpos(d))*(oldpos(d) - tpos(d));
}
dist = sqrt(dist);
Vtest(0) = 1/dist;
Array1 <doublevar> R2(5);
for(int i=1; i< nelectrons; i++) {
sample->getEEDist(0,i,R2);
Vtest(i)= 1/R2(0);
}
sample->setElectronPosNoNotify(0, oldpos);
sample->updateEIDist();
sample->updateEEDist();
//cout << "elec-elec: " << elecElec << endl;
//cout << "pot " << pot << endl;
for(int d=0; d< 3; d++)
Vtest(nelectrons) -=electric_field(d)*tpos(d);
}
void FirstOrderType1RTest::testBuildFirstOrderType1R2()
{
std::cout << "--> Test: constructor data (2)." <<std::endl;
SP::FirstOrderType1R R2(new FirstOrderType1R("TestPlugin:hT1", "TestPlugin:gT1", "TestPlugin:Jh0T1", "TestPlugin:Jg0T1"));
CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2b : ", R2->getType() == RELATION::FirstOrder, true);
CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2c : ", R2->getSubType() == RELATION::Type1R, true);
// CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2d : ", R2->gethName()=="TestPlugin:hT1", true);
// CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2e : ", R2->getgName()=="TestPlugin:gT1", true);
// CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2e : ", R2->getJachName(0)=="TestPlugin:Jh0T1", true);
// CPPUNIT_ASSERT_EQUAL_MESSAGE("testBuildFirstOrderType1R2g : ", R2->getJacgName(0)=="TestPlugin:Jg0T1", true);
std::cout << "--> Constructor2 test ended with success." <<std::endl;
}
//--------------------------------------------decomposeProhMats-------------------------------------------//
// through the projection matrix we can get nearly all the information of the cameras
// the position of the camera
// the direction of the camera
// the focal of the camera
// the axises of the camera
void Camera::decomposeProjMats()
{
// 1.0 get direction
this->dir_.at<float>(0) = this->project_.at<float>(2,0);
this->dir_.at<float>(1) = this->project_.at<float>(2,1);
this->dir_.at<float>(2) = this->project_.at<float>(2,2);
this->dir_ = this->dir_/ norm(this->dir_);
// 2.0 get position
cv::Mat KR(3,3,CV_32FC1);
cv::Mat KT(3,1,CV_32FC1);
for(int i=0; i<3; i++)
{
for(int j=0; j<3; j++)
{
KR.at<float>(i,j) = this->project_.at<float>(i,j);
}
}
for(int i=0; i<3; i++)
KT.at<float>(i,0) = this->project_.at<float>(i,3);
this->pos_ = -KR.inv()* KT;
// 3.0 compute the focal
cv::Mat R0(3,1, CV_32FC1);
cv::Mat R1(3, 1, CV_32FC1);
cv::Mat R2(3, 1, CV_32FC1);
for(int i=0; i<3; i++)
{
R0.at<float>(i) = KR.at<float>(0, i);
R1.at<float>(i) = KR.at<float>(1, i);
R2.at<float>(i) = KR.at<float>(2, i);
}
this->focal_ = 0.5*abs(norm(R0.cross(R2)))+ 0.5*abs(norm(R1.cross(R2)));
// 4.0 axises of the camera
this->zaxis_ = this->dir_;
this->yaxis_ = this->zaxis_.cross(R0);
this->yaxis_ = this->yaxis_/norm(this->yaxis_);
this->xaxis_ = this->yaxis_.cross(this->zaxis_);
this->xaxis_ = this->xaxis_/norm(this->xaxis_);
KR.release();
KT.release();
R0.release();
R1.release();
R2.release();
}
TEST(TestBookingCalendar, testDuplicateReservation) {
BookingCalendar calendar(2015);
Room R1(1, 20, 300, 400, 405), R2(2, 15, 200, 300, 302);
calendar.reserveRoom(R1, 2015, 12, 13);
calendar.reserveRoom(R2, 2015, 12, 13);
try {
calendar.reserveRoom(R2, 2015, 12, 13);
} catch (const RoomIsAlreadyRegisteredOnThisDayException & err) {
ASSERT_STREQ("Room with id#2 is already registered in day#13", err.what());
}
}
TypeOfFE_P2ttdc(): TypeOfFE(0,0,6,1,Data,3,1,6,6,Pi_h_coef)
{ const R2 Pt[] = { Shrink(R2(0,0)), Shrink(R2(1,0)), Shrink(R2(0,1)),
Shrink(R2(0.5,0.5)),Shrink(R2(0,0.5)),Shrink(R2(0.5,0)) };
for (int i=0;i<NbDoF;i++) {
pij_alpha[i]= IPJ(i,i,0);
P_Pi_h[i]=Pt[i]; }
}
template <class Type,class TypeB> void bench_r2d_assoc
(
const OperAssocMixte & op,
Type *,
TypeB *,
Pt2di sz,
INT x0,
INT x1
)
{
Im2D<Type,TypeB> I(sz.x,sz.y,(Type)0);
Im2D_REAL8 R1(sz.x,sz.y,0.0);
Im2D_REAL8 R2(sz.x,sz.y,0.0);
Im2D_REAL8 R3(sz.x,sz.y,0.0);
ELISE_COPY(I.all_pts(),255*frandr(),I.out());
ELISE_COPY
(
I.all_pts(),
linear_red(op,I.in(),x0,x1),
R1.out()
);
TypeB vdef;
op.set_neutre(vdef);
ELISE_COPY
(
I.all_pts(),
rect_red(op,I.in(vdef),Box2di(Pt2di(x0,0),Pt2di(x1,0))),
R2.out()
);
ELISE_COPY
(
I.lmr_all_pts(Pt2di(1,0)),
linear_red(op,I.in(),x0,x1),
R3.out()
);
REAL d12,d23;
ELISE_COPY
(
R1.all_pts(),
Virgule(
Abs(R1.in()-R2.in()),
Abs(R2.in()-R3.in())
),
Virgule(VMax(d12),VMax(d23))
);
BENCH_ASSERT((d12<epsilon)&&(d23<epsilon));
}
//#define DEBUG
void SymList::compute_subgroup()
{
Matrix2D<DOUBLE> I(4, 4);
I.initIdentity();
Matrix2D<DOUBLE> L1(4, 4), R1(4, 4), L2(4, 4), R2(4, 4), newL(4, 4), newR(4, 4);
Matrix2D<int> tried(true_symNo, true_symNo);
int i, j;
int new_chain_length;
while (found_not_tried(tried, i, j, true_symNo))
{
tried(i, j) = 1;
get_matrices(i, L1, R1);
get_matrices(j, L2, R2);
newL = L1 * L2;
newR = R1 * R2;
new_chain_length = __chain_length(i) + __chain_length(j);
Matrix2D<DOUBLE> newR3 = newR;
newR3.resize(3,3);
if (newL.isIdentity() && newR3.isIdentity()) continue;
// Try to find it in current ones
bool found;
found = false;
for (int l = 0; l < SymsNo(); l++)
{
get_matrices(l, L1, R1);
if (newL.equal(L1) && newR.equal(R1))
{
found = true;
break;
}
}
if (!found)
{
//#define DEBUG
#ifdef DEBUG
std::cout << "Matrix size " << tried.Xdim() << " "
<< "trying " << i << " " << j << " "
<< "chain length=" << new_chain_length << std::endl;
std::cout << "Result R Sh\n" << newR;
#endif
#undef DEBUG
newR.setSmallValuesToZero();
newL.setSmallValuesToZero();
add_matrices(newL, newR, new_chain_length);
tried.resize(MAT_YSIZE(tried) + 1, MAT_XSIZE(tried) + 1);
}
}
}
void revolute_joint_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mEnd) || (!mBase))
return;
mEnd->Parent = mBase->Parent;
mEnd->Position = mBase->Position;
mEnd->Velocity = mBase->Velocity;
mEnd->Acceleration = mBase->Acceleration;
if(!mAngle) {
mEnd->Quat = mBase->Quat;
mEnd->AngVelocity = mBase->AngVelocity;
mEnd->AngAcceleration = mBase->AngAcceleration;
} else {
quaternion<double> tmp_quat(axis_angle<double>(mAngle->q,mAxis).getQuaternion());
rot_mat_3D<double> R2(tmp_quat.getRotMat());
mEnd->Quat = mBase->Quat * tmp_quat;
mEnd->AngVelocity = (mBase->AngVelocity * R2) + mAngle->q_dot * mAxis;
mEnd->AngAcceleration = (mBase->AngAcceleration * R2) + ((mBase->AngVelocity * R2) % (mAngle->q_dot * mAxis)) + mAngle->q_ddot * mAxis;
if(mJacobian) {
mJacobian->Parent = mEnd;
mJacobian->qd_vel = vect<double,3>();
mJacobian->qd_avel = mAxis;
mJacobian->qd_acc = vect<double,3>();
mJacobian->qd_aacc = vect<double,3>();
};
};
mEnd->UpdateQuatDot();
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mBase]))
aStorage->frame_3D_mapping[mBase] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mBase)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mBase])) = (*mBase);
if(!(aStorage->frame_3D_mapping[mEnd]))
aStorage->frame_3D_mapping[mEnd] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mEnd)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mEnd])) = (*mEnd);
if(mAngle) {
if(!(aStorage->gen_coord_mapping[mAngle]))
aStorage->gen_coord_mapping[mAngle] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mAngle)),scoped_deleter());
else
(*(aStorage->gen_coord_mapping[mAngle])) = (*mAngle);
};
};
};
int main()
{
Rational R1(18,45);
R1.print1();
R1.print2();
Rational R2(24,64);
R2.print1();
R2.print2();
Rational R3 = R1.add(R2);
R3.print1();
R3.print2();
return 0;
}
void Node::drawSimple(QPainter &P, MapView *theView)
{
// if (!M_PREFS->getWireframeView() && !TEST_RFLAGS(RendererOptions::Interacting))
// return;
if (! ((isReadonly() || !isSelectable(theView->pixelPerM(), theView->renderOptions())) && (!isPOI() && !isWaypoint())))
// if (!Pt->isReadonly() && Pt->isSelectable(r))
{
if (!layer()) {
qDebug() << "Node without layer: " << id().numId << xmlId();
return;
}
qreal WW = theView->nodeWidth();
if (WW >= 1) {
QColor theColor = QColor(0,0,0,128);
if (M_PREFS->getUseStyledWireframe() && hasPainter()) {
const FeaturePainter* thePainter = getCurrentPainter();
if (thePainter->DrawForeground)
theColor = thePainter->ForegroundColor;
else if (thePainter->DrawBackground)
theColor = thePainter->BackgroundColor;
}
QPointF Pp(theView->toView(this));
if (layer()->classGroups() & Layer::Special) {
QRect R2(Pp.x()-(WW+4)/2, Pp.y()-(WW+4)/2, WW+4, WW+4);
P.fillRect(R2,QColor(255,0,255,192));
} else if (isWaypoint()) {
QRect R2(Pp.x()-(WW+4)/2, Pp.y()-(WW+4)/2, WW+4, WW+4);
P.fillRect(R2,QColor(255,0,0,192));
}
QRect R(Pp.x()-WW/2, Pp.y()-WW/2, WW, WW);
P.fillRect(R,theColor);
}
}
}
self& addBefore(const base& aPose) {
rot_mat_3D<value_type> R(this->Quat.getRotMat());
this->Position += R * aPose.Position;
Velocity += R * ( AngVelocity % aPose.Position );
Acceleration += R * ( (AngVelocity % (AngVelocity % aPose.Position)) + (AngAcceleration % aPose.Position) );
rot_mat_3D<value_type> R2(aPose.Quat.getRotMat());
this->Quat *= aPose.Quat;
AngAcceleration = (AngAcceleration * R2);
AngVelocity = (AngVelocity * R2);
UpdateQuatDot();
return *this;
};
/**
* Adds frame Frame_ before this coordinate frame ("before" is meant in the same sense as for pose_3D::addBefore()).
* The transformation uses classic "rotating frame" formulae.
* \note No operations are performed on forces.
*/
self& addBefore(const self& aFrame) {
rot_mat_3D<value_type> R(this->Quat.getRotMat());
this->Position += R * aFrame.Position;
Velocity += R * ( (AngVelocity % aFrame.Position) + aFrame.Velocity );
Acceleration += R * ( (AngVelocity % (AngVelocity % aFrame.Position)) + T(2.0) * (AngVelocity % aFrame.Velocity) + (AngAcceleration % aFrame.Position) + aFrame.Acceleration );
rot_mat_3D<value_type> R2(aFrame.Quat.getRotMat());
this->Quat *= aFrame.Quat;
AngAcceleration = (AngAcceleration * R2) + ((AngVelocity * R2) % aFrame.AngVelocity) + aFrame.AngAcceleration;
AngVelocity = (AngVelocity * R2) + aFrame.AngVelocity;
UpdateQuatDot();
return *this;
};
void prueba15 ()
{ //--------------------------- begin -------------------------------
std::vector<uint64_t> A, B, C ( 30 , 0) ;
for ( uint32_t i =0 ; i < 10 ; ++i)
{ A.push_back ( 100+i);
B.push_back ( 80 +i);
};
bs_util::range<uint64_t*> R1 ( &A[0], &A[10]), R2 ( &B[0], &B[10]);
uninit_full_merge (&C[0],R1,R2, std::less<uint64_t>() );
for ( uint32_t i =0 ; i < 10 ; ++i)
BOOST_CHECK ( C[i] == 80+i);
for ( uint32_t i =10 ; i < 20 ; ++i)
BOOST_CHECK ( C[i] == 90+i) ;
}
// [[Rcpp::export]]
VectorXd bootR2(const MatrixXd X, const VectorXd y, int nBoot){
RNGScope scope;
const int n(X.rows());
// const int p(X.cols());
VectorXd R2s(nBoot);
MatrixXd Xi(X);
VectorXd yi(y);
IntegerVector prm(n);
// double R2i(R2(Xi, yi));
for(int i = 0; i < nBoot; ++i) {
prm = bootPerm(n);
Xi = shuffleMatrix(X, prm);
yi = shuffleVector(y, prm);
R2s(i) = R2(Xi, yi);
}
return R2s;
}
geometry_msgs::Pose Sensors::robot2sensorTransformation(geometry_msgs::Pose pose)
{
Eigen::Matrix4d robotPoseMat, robot2sensorMat, sensorPoseMat;
//Robot matrix pose
Eigen::Matrix3d R; Eigen::Vector3d T1(pose.position.x,pose.position.y,pose.position.z);
tf::Quaternion qt(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
tf::Matrix3x3 R1(qt);
tf::matrixTFToEigen(R1,R);
robotPoseMat.setZero ();
robotPoseMat.block (0, 0, 3, 3) = R;
robotPoseMat.block (0, 3, 3, 1) = T1;
robotPoseMat (3, 3) = 1;
//transformation matrix
qt = tf::createQuaternionFromRPY(sensorRPY[0],sensorRPY[1],sensorRPY[2]);
tf::Matrix3x3 R2(qt);Eigen::Vector3d T2(sensorPose[0], sensorPose[1], sensorPose[2]);
tf::matrixTFToEigen(R2,R);
robot2sensorMat.setZero ();
robot2sensorMat.block (0, 0, 3, 3) = R;
robot2sensorMat.block (0, 3, 3, 1) = T2;
robot2sensorMat (3, 3) = 1;
//preform the transformation
sensorPoseMat = robotPoseMat * robot2sensorMat;
Eigen::Matrix4d sensor2sensorMat; //the frustum culling sensor needs this
//the transofrmation is rotation by +90 around x axis of the sensor
sensor2sensorMat << 1, 0, 0, 0,
0, 0,-1, 0,
0, 1, 0, 0,
0, 0, 0, 1;
Eigen::Matrix4d newSensorPoseMat = sensorPoseMat * sensor2sensorMat;
geometry_msgs::Pose p;
Eigen::Vector3d T3;Eigen::Matrix3d Rd; tf::Matrix3x3 R3;
Rd = newSensorPoseMat.block (0, 0, 3, 3);
tf::matrixEigenToTF(Rd,R3);
T3 = newSensorPoseMat.block (0, 3, 3, 1);
p.position.x=T3[0];p.position.y=T3[1];p.position.z=T3[2];
R3.getRotation(qt);
p.orientation.x = qt.getX(); p.orientation.y = qt.getY();p.orientation.z = qt.getZ();p.orientation.w = qt.getW();
return p;
}
/**
* Adds frame Frame_ after this coordinate frame ("after" is meant in the same sense as for pose_3D::addAfter()).
* The transformation uses classic "rotating frame" formulae.
* \note No operations are performed on forces.
*/
self& addAfter(const self& aFrame) {
rot_mat_3D<value_type> R(aFrame.Quat.getRotMat());
Acceleration = aFrame.Acceleration + R * ( (aFrame.AngVelocity % (aFrame.AngVelocity % this->Position)) + value_type(2.0) * (aFrame.AngVelocity % Velocity) + (aFrame.AngAcceleration % this->Position) + Acceleration );
Velocity = aFrame.Velocity + R * ( (aFrame.AngVelocity % this->Position) + Velocity );
this->Position = aFrame.Position + R * this->Position;
rot_mat_3D<value_type> R2(this->Quat.getRotMat());
AngAcceleration += (aFrame.AngAcceleration * R2) + ((aFrame.AngVelocity * R2) % AngVelocity);
AngVelocity += (aFrame.AngVelocity * R2);
this->Quat = aFrame.Quat * this->Quat;
this->Parent = aFrame.Parent;
UpdateQuatDot();
return *this;
};
void FFTMulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
if (IsZero(a) || IsZero(b)) {
clear(x);
return;
}
long d = deg(a) + deg(b);
if (n > d + 1)
n = d + 1;
long k = NextPowerOfTwo(d + 1);
fftRep R1(INIT_SIZE, k), R2(INIT_SIZE, k);
TofftRep(R1, a, k);
TofftRep(R2, b, k);
mul(R1, R1, R2);
FromfftRep(x, R1, 0, n-1);
}
void
Adjoint::construct(const Rotation& R, const Translation& T)
{
_data = arma::zeros<arma::mat>(6, 6);
arma::colvec::fixed<3> v;
v(0) = T.at(0); v(1) = T.at(1); v(2) = T.at(2);
Skew S(v);
arma::mat::fixed<3, 3> R2 = S._data*R._data;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
{
_data(i, j + 3) = R2(i, j);
_data(i, j) = _data(i + 3, j + 3) = R.at(i, j);
}
_R = R;
_T = T;
}
void Hand_recognition::Detect_Skin(IplImage *src, IplImage *dst){
cvCvtColor(src, img_YCrCb, CV_BGR2YCrCb);
cvCvtColor(src, img_HSV, CV_BGR2HSV);
cvZero(dst);
for(int i = 0; i < dst->height; i++){
for(int j = 0; j < dst->width; j++){
B = (unsigned char)src->imageData[(j * 3) + i * src->widthStep];
G = (unsigned char)src->imageData[(j * 3) + i * src->widthStep + 1];
R = (unsigned char)src->imageData[(j * 3) + i * src->widthStep + 2];
bool a = R1(R,G,B);
if(a){
H = (unsigned char)img_HSV->imageData[(j * 3) + i * img_HSV->widthStep];
S = (unsigned char)img_HSV->imageData[(j * 3) + i * img_HSV->widthStep + 1];
V = (unsigned char)img_HSV->imageData[(j * 3) + i * img_HSV->widthStep + 2];
bool c = R3(H,S,V);;
if(c){
Y = (unsigned char)img_YCrCb->imageData[(j * 3) + i * img_YCrCb->widthStep];
Cr = (unsigned char)img_YCrCb->imageData[(j * 3) + i * img_YCrCb->widthStep + 1];
Cb = (unsigned char)img_YCrCb->imageData[(j * 3) + i * img_YCrCb->widthStep + 2];
bool b = R2(Y,Cr,Cb);
if(b)
dst->imageData[j + i * dst->widthStep] = (unsigned char) 255;
}
}
}
}
cvErode(dst, dst, 0, MOP_NUM);
cvDilate(dst, dst, 0, MOP_NUM);
}
template <class Type,class TypeB> void bench_r2d_lin_cumul
(
const OperAssocMixte & op,
Type *,
TypeB *,
Pt2di sz
)
{
Im2D<Type,TypeB> I(sz.x,sz.y,(Type)0);
Im2D_REAL8 R1(sz.x,sz.y,(REAL8)0);
Im2D_REAL8 R2(sz.x,sz.y,(REAL8)0);
ELISE_COPY(I.all_pts(),255*frandr(),I.out());
ELISE_COPY(I.all_pts(),lin_cumul_ass(op,I.in()),R1.out());
ELISE_COPY(I.lmr_all_pts(Pt2di(1,0)),lin_cumul_ass(op,I.in()),R2.out());
REAL dif;
ELISE_COPY (R1.all_pts(),Abs(R1.in()-R2.in()),VMax(dif));
BENCH_ASSERT(dif<epsilon);
}
void DrawJoint(Joint* joint)
{
Body* b1 = joint->body1;
Body* b2 = joint->body2;
Mat22 R1(b1->rotation);
Mat22 R2(b2->rotation);
Vec2 x1 = b1->position;
Vec2 p1 = x1 + R1 * joint->localAnchor1;
Vec2 x2 = b2->position;
Vec2 p2 = x2 + R2 * joint->localAnchor2;
glColor3f(0.5f, 0.5f, 0.8f);
glBegin(GL_LINES);
glVertex2f(x1.x, x1.y);
glVertex2f(p1.x, p1.y);
glVertex2f(x2.x, x2.y);
glVertex2f(p2.x, p2.y);
glEnd();
}
void Problem3ComputeItemIlana::RecomputeData(const DataBoxAccess& box) const {
// Get coordinates from DataBox
const MyVector<DataMesh>& coords = box.Get<MyVector<DataMesh> >("GlobalCoords");
const Mesh& mesh = coords[0];
// Calculate R^2, which is the square of the magnitude of the coordinates GlobalCoords
DataMesh R2(mesh,0.0);
for(int i=0;i<coords.Size();++i)
R2 += coords[i]*coords[i];
DataMesh R = sqrt(R2);
// Make sure that R is not zero
for(int i=0;i<R2.Size();++i)
REQUIRE(R[i]!=0,"R is zero! Can't divide by zero!");
// Calculate rhat^i/r^3
Tensor<DataMesh> tmp(3, "a", mesh, 0.0);
for(int i=0;i<coords.Size();++i)
tmp(i) = coords[i]/(R*R*R);
mResult.assign(tmp);
}
void rotate(char axis, value_type angle)
{
const value_type rad = angle * (M_PI / 180);
const value_type cosValue = cos(rad);
const value_type sinValue = sin(rad);
Vector4D R1(1.f, 0.f, 0.f, 0.f);
Vector4D R2(0.f, 1.f, 0.f, 0.f);
Vector4D R3(0.f, 0.f, 1.f, 0.f);
const Vector4D R4(0.f, 0.f, 0.f, 1.f);
switch (axis)
{
case 'x':
case 'X':
R2[1] = cosValue;
R2[2] = -sinValue;
R3[1] = sinValue;
R3[2] = cosValue;
break;
case 'y':
case 'Y':
R1[0] = cosValue;
R1[2] = sinValue;
R3[0] = -sinValue;
R3[2] = cosValue;
break;
case 'z':
case 'Z':
R1[0] = cosValue;
R1[1] = -sinValue;
R2[0] = sinValue;
R2[1] = cosValue;
break;
}
(*this) = (*this) * Matrix4x4(R1, R2, R3, R4);
}
int cBoxJoint::GetPointList(D3DXVECTOR2 pOut[])
{
Body* b1 = this->body1;
Body* b2 = this->body2;
Mat22 R1(b1->rotation);
Mat22 R2(b2->rotation);
Vec2 x1 = b1->position;
//Vec2 p1 = x1 + R1 * this->localAnchor1;
Vec2 x2 = b2->position;
//Vec2 p2 = x2 + R2 * this->localAnchor2;
pOut[0].x = x1.x;
pOut[0].y = x1.y;
pOut[1].x = x2.x;
pOut[1].y = x2.y;
return 0;
}
void extr(jvec &ext_EP,jvec &ext_ED,jvec &ext_Q2,jvec &ext_fP,jvec &ext_fM,jvec &ext_f0,jvec &ext_fT,int il_sea,int il,int ic)
{
////////////////////////////////////////// R0 //////////////////////////////////////
jvec R0_corr;
jack R0(njack);
//load standing
jvec ll0_st=load_3pts("V0",il,il,0,RE,ODD,1);
jvec lc0_st=load_3pts("V0",ic,il,0,RE,ODD,1);
jvec cc0_st=load_3pts("V0",ic,ic,0,RE,ODD,1);
//build R0
R0_corr=lc0_st*lc0_st.simmetric()/(cc0_st*ll0_st);
//fit and plot
R0=constant_fit(R0_corr,TH-tmax,tmax,combine("plots/R0_il_%d_ic_%d.xmg",il,ic).c_str());
//////////////////////////////////////////// R2 ////////////////////////////////////
jvec R2_corr[nth];
jvec RT_corr[nth];
jvec R2(nth,njack);
jvec RT(nth,njack);
ofstream out_R2(combine("plots/R2_il_%d_ic_%d.xmg",il,ic).c_str());
ofstream out_RT(combine("plots/RT_il_%d_ic_%d.xmg",il,ic).c_str());
jvec lcK_th[nth],lc0_th[nth],lcT_th[nth];
for(int ith=0;ith<nth;ith++)
{
//load corrs
lcK_th[ith]=load_3pts("VK",ic,il,ith,IM,EVN,-1)/(6*th_P[ith]);
lc0_th[ith]=load_3pts("V0",ic,il,ith,RE,ODD,1);
lcT_th[ith]=load_3pts("VTK",ic,il,ith,IM,ODD,1)/(6*th_P[ith]);
//build ratios
R2_corr[ith]=lcK_th[ith]/lc0_th[ith];
RT_corr[ith]=lcT_th[ith]/lcK_th[ith];
//fit
R2[ith]=constant_fit(R2_corr[ith],tmin,tmax);
RT[ith]=constant_fit(RT_corr[ith],tmin,tmax);
//plot
out_R2<<write_constant_fit_plot(R2_corr[ith],R2[ith],tmin,tmax);
out_RT<<write_constant_fit_plot(RT_corr[ith],RT[ith],tmin,tmax);
}
////////////////////////////////////////// R1 //////////////////////////////////////
jvec R1_corr[nth];
jvec R1(nth,njack);
ofstream out_P(combine("plots/out_P_il_%d_ic_%d.xmg",il,ic).c_str());
out_P<<"@type xydy"<<endl;
ofstream out_D(combine("plots/out_D_il_%d_ic_%d.xmg",il,ic).c_str());
out_D<<"@type xydy"<<endl;
ofstream out_R1(combine("plots/out_R1_il_%d_ic_%d.xmg",il,ic).c_str());
out_R1<<"@type xydy"<<endl;
//load Pi and D
jvec P_corr[nth],D_corr[nth];
jvec ED(nth,njack),EP(nth,njack);
for(int ith=0;ith<nth;ith++)
{
//load moving pion
P_corr[ith]=load_2pts("2pts_P5P5.dat",il_sea,il,ith);
out_P<<"@type xydy"<<endl;
EP[ith]=constant_fit(effective_mass(P_corr[ith]),tmin_P,TH,combine("plots/P_eff_mass_il_%d_ic_%d_ith_%d.xmg",
il,ic,ith).c_str());
out_P<<write_constant_fit_plot(effective_mass(P_corr[ith]),EP[ith],tmin_P,TH);
out_P<<"&"<<endl;
//recompute EP and ED from standing one
if(ith)
{
ED[ith]=latt_en(ED[0],th_P[ith]);
EP[ith]=latt_en(EP[0],th_P[ith]);
}
//load moving D
D_corr[ith]=load_2pts("2pts_P5P5.dat",il,ic,ith);
out_D<<"@type xydy"<<endl;
ED[ith]=constant_fit(effective_mass(D_corr[ith]),tmin_D,TH,combine("plots/D_eff_mass_il_%d_ic_%d_ith_%d.xmg",
il,ic,ith).c_str());
out_D<<write_constant_fit_plot(effective_mass(D_corr[ith]),ED[ith],tmin_D,TH);
out_D<<"&"<<endl;
//build the ratio
R1_corr[ith]=lc0_th[ith]/lc0_th[0];
for(int t=0;t<TH;t++)
{
int E_fit_reco_flag=1;
jack Dt(njack),Pt(njack);
if(E_fit_reco_flag==0)
{
Dt=D_corr[0][t]/D_corr[ith][t];
Pt=P_corr[0][TH-t]/P_corr[ith][TH-t];
}
else
{
jack ED_th=latt_en(ED[0],th_P[ith]),EP_th=latt_en(EP[0],th_P[ith]);
Dt=exp(-(ED[0]-ED_th)*t)*ED_th/ED[0];
Pt=exp(-(EP[0]-EP_th)*(TH-t))*EP_th/EP[0];
}
R1_corr[ith][t]*=Dt*Pt;
}
//fit
R1[ith]=constant_fit(R1_corr[ith],tmin,tmax);
//plot
out_R1<<write_constant_fit_plot(R1_corr[ith],R1[ith],tmin,tmax);
}
//////////////////////////////////////// solve the ratios //////////////////////////////
//compute f0[q2max]
jvec f0_r(nth,njack),fP_r(nth,njack),fT_r(nth,njack);
f0_r[0]=sqrt(R0*4*ED[0]*EP[0])/(ED[0]+EP[0]);
cout<<"f0_r[q2max]: "<<f0_r[0]<<endl;
//compute QK and Q2
double mom[nth];
jvec PK(nth,njack),QK(nth,njack);
jvec P0(nth,njack),Q0(nth,njack),Q2(nth,njack),P2(nth,njack);
jvec P0_r(nth,njack),Q0_r(nth,njack),Q2_r(nth,njack),P2_r(nth,njack);
for(int ith=0;ith<nth;ith++)
{
P0[ith]=ED[ith]+EP[ith]; //P=initial+final
Q0[ith]=ED[ith]-EP[ith]; //Q=initial-final
P0_r[ith]=latt_en(ED[0],th_P[ith])+latt_en(EP[0],th_P[ith]);
Q0_r[ith]=latt_en(ED[0],th_P[ith])-latt_en(EP[0],th_P[ith]);
//we are describing the process D->Pi
mom[ith]=momentum(th_P[ith]);
double P_D=-mom[ith];
double P_Pi=mom[ith];
PK[ith]=P_D+P_Pi;
QK[ith]=P_D-P_Pi;
P2[ith]=sqr(P0[ith])-3*sqr(PK[ith]);
Q2[ith]=sqr(Q0[ith])-3*sqr(QK[ith]);
//reconstruct Q2
P2_r[ith]=sqr(P0_r[ith])-3*sqr(PK[ith]);
Q2_r[ith]=sqr(Q0_r[ith])-3*sqr(QK[ith]);
}
//checking Pion dispertion relation
ofstream out_disp_P(combine("plots/Pion_disp_rel_il_%d_ic_%d.xmg",il,ic).c_str());
out_disp_P<<"@type xydy"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(EP[ith])<<endl;
out_disp_P<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(cont_en(EP[0],th_P[ith]))<<endl;
out_disp_P<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(latt_en(EP[0],th_P[ith]))<<endl;
out_disp_P<<"&"<<endl;
//checking D dispertion relation
ofstream out_disp_D(combine("plots/D_disp_rel_il_%d_ic_%d.xmg",il,ic).c_str());
out_disp_D<<"@type xydy"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(ED[ith])<<endl;
out_disp_D<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(cont_en(ED[0],th_P[ith]))<<endl;
out_disp_D<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(latt_en(ED[0],th_P[ith]))<<endl;
out_disp_D<<"&"<<endl;
//compute xi
jvec xi(nth,njack);
for(int ith=1;ith<nth;ith++)
{
int E_fit_reco_flag=0; //it makes no diff
jack P0_th=E_fit_reco_flag?P0_r[ith]:P0[ith];
jack Q0_th=E_fit_reco_flag?Q0_r[ith]:Q0[ith];
xi[ith]=R2[ith]*P0_th;
xi[ith]/=QK[ith]-R2[ith]*Q0_th;
}
//compute fP
ofstream out_fP_r(combine("plots/fP_r_il_%d_ic_%d.xmg",il,ic).c_str());
out_fP_r<<"@type xydy"<<endl;
for(int ith=1;ith<nth;ith++)
{
int E_fit_reco_flag=1; //it makes no diff
jack P0_th=E_fit_reco_flag?P0_r[ith]:P0[ith];
jack Q0_th=E_fit_reco_flag?Q0_r[ith]:Q0[ith];
jack c=P0_th/(ED[0]+EP[0])*(1+xi[ith]*Q0_th/P0_th);
fP_r[ith]=R1[ith]/c*f0_r[0];
out_fP_r<<Q2[ith].med()<<" "<<fP_r[ith]<<endl;
}
//compute f0 and fT
ofstream out_f0_r(combine("plots/f0_r_il_%d_ic_%d.xmg",il,ic).c_str());
ofstream out_fT_r(combine("plots/fT_r_il_%d_ic_%d.xmg",il,ic).c_str());;
out_f0_r<<"@type xydy"<<endl;
out_f0_r<<Q2[0].med()<<" "<<f0_r[0]<<endl;
out_fT_r<<"@type xydy"<<endl;
for(int ith=1;ith<nth;ith++)
{
//it seems better here to solve using reconstructed energies
int E_fit_reco_flag=0;
jack EP_th=E_fit_reco_flag?latt_en(EP[0],th_P[ith]):EP[ith];
jack ED_th=E_fit_reco_flag?latt_en(ED[0],th_P[ith]):ED[ith];
jack Q2_th=E_fit_reco_flag?Q2_r[ith]:Q2[ith];
jack fM_r=xi[ith]*fP_r[ith]; //checked
f0_r[ith]=fP_r[ith]+fM_r[ith]*Q2_th/(sqr(ED_th)-sqr(EP_th));
out_f0_r<<Q2[ith].med()<<" "<<f0_r[ith]<<endl;
fT_r[ith]=fM_r[ith]*RT[ith]*Zt_med[ibeta]/Zv_med[ibeta]*(EP[0]+ED[0])/(ED[ith]+EP[ith]); //ADD
out_fT_r<<Q2[ith].med()<<" "<<fT_r[ith]<<endl;
}
//////////////////////////////////////// analytic method /////////////////////////////
jvec fP_a(nth,njack),fM_a(nth,njack),f0_a(nth,njack),fT_a(nth,njack);
jvec fP_n(nth,njack),fM_n(nth,njack),f0_n(nth,njack),fT_n(nth,njack);
//determine M and Z for pion and D
jvec ZP(nth,njack),ZD(nth,njack);
for(int ith=0;ith<nth;ith++)
{
jack E,Z2;
two_pts_fit(E,Z2,P_corr[ith],tmin_P,TH);
ZP[ith]=sqrt(Z2);
two_pts_fit(E,Z2,D_corr[ith],tmin_D,TH);
ZD[ith]=sqrt(Z2);
}
//compute V
jvec VK_a(nth,njack),V0_a(nth,njack),TK_a(nth,njack);
jvec VK_n(nth,njack),V0_n(nth,njack),TK_n(nth,njack);
for(int ith=0;ith<nth;ith++)
{
ofstream out_V0(combine("plots/V0_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_V0<<"@type xydy"<<endl;
ofstream out_VK(combine("plots/VK_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_VK<<"@type xydy"<<endl;
ofstream out_TK(combine("plots/TK_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_TK<<"@type xydy"<<endl;
ofstream out_dt(combine("plots/dt_il_%d_ic_%d_ith_%d.xmg",il,ic,ith).c_str());
out_dt<<"@type xydy"<<endl;
//computing time dependance
jvec dt_a(TH+1,njack),dt_n(TH+1,njack);
{
//it seems better here to use fitted energies
int E_fit_reco_flag=1;
jack EP_th=E_fit_reco_flag?latt_en(EP[0],th_P[ith]):EP[ith];
jack ED_th=E_fit_reco_flag?latt_en(ED[0],th_P[ith]):ED[ith];
for(int t=0;t<=TH;t++)
{
dt_a[t]=exp(-(ED_th*t+EP_th*(TH-t)))*ZP[0]*ZD[0]/(4*EP_th*ED_th);
dt_n[t]=D_corr[ith][t]*P_corr[ith][TH-t]/(ZD[0]*ZP[0]);
}
}
//remove time dependance using analytic or numeric expression
jvec VK_corr_a=Zv_med[ibeta]*lcK_th[ith]/dt_a,V0_corr_a=Zv_med[ibeta]*lc0_th[ith]/dt_a;
jvec VK_corr_n=Zv_med[ibeta]*lcK_th[ith]/dt_n,V0_corr_n=Zv_med[ibeta]*lc0_th[ith]/dt_n;
jvec TK_corr_n=Zt_med[ibeta]*lcT_th[ith]/dt_n,TK_corr_a=Zt_med[ibeta]*lcT_th[ith]/dt_a;
//fit V0
V0_a[ith]=constant_fit(V0_corr_a,tmin,tmax);
V0_n[ith]=constant_fit(V0_corr_n,tmin,tmax);
out_V0<<write_constant_fit_plot(V0_corr_a,V0_a[ith],tmin,tmax)<<"&"<<endl;
out_V0<<write_constant_fit_plot(V0_corr_n,V0_n[ith],tmin,tmax)<<"&"<<endl;
//fit VK
VK_a[ith]=constant_fit(VK_corr_a,tmin,tmax);
VK_n[ith]=constant_fit(VK_corr_n,tmin,tmax);
out_VK<<write_constant_fit_plot(VK_corr_a,VK_a[ith],tmin,tmax)<<"&"<<endl;
out_VK<<write_constant_fit_plot(VK_corr_n,VK_n[ith],tmin,tmax)<<"&"<<endl;
//fit TK
TK_a[ith]=constant_fit(TK_corr_a,tmin,tmax);
TK_n[ith]=constant_fit(TK_corr_n,tmin,tmax);
out_TK<<write_constant_fit_plot(TK_corr_a,TK_a[ith],tmin,tmax)<<"&"<<endl;
out_TK<<write_constant_fit_plot(TK_corr_n,TK_n[ith],tmin,tmax)<<"&"<<endl;
}
//compute f0(q2max)
f0_a[0]=V0_a[0]/(ED[0]+EP[0]);
f0_n[0]=V0_n[0]/(ED[0]+EP[0]);
cout<<"f0_a["<<Q2[0].med()<<"]: "<<f0_a[0]<<endl;
cout<<"f0_n["<<Q2[0].med()<<"]: "<<f0_n[0]<<endl;
//solve for fP and f0
for(int ith=1;ith<nth;ith++)
{
jack delta=P0[ith]*QK[ith]-Q0[ith]*PK[ith];
//solve using analytic fit
jack deltaP_a=V0_a[ith]*QK[ith]-Q0[ith]*VK_a[ith];
jack deltaM_a=P0[ith]*VK_a[ith]-V0_a[ith]*PK[ith];
fP_a[ith]=deltaP_a/delta;
fM_a[ith]=deltaM_a/delta;
//solve using numeric fit
jack deltaP_n=V0_n[ith]*QK[ith]-Q0[ith]*VK_n[ith];
jack deltaM_n=P0[ith]*VK_n[ith]-V0_n[ith]*PK[ith];
fP_n[ith]=deltaP_n/delta;
fM_n[ith]=deltaM_n/delta;
//compute f0
f0_a[ith]=fP_a[ith]+fM_a[ith]*Q2[ith]/(ED[0]*ED[0]-EP[0]*EP[0]);
f0_n[ith]=fP_n[ith]+fM_n[ith]*Q2[ith]/(ED[0]*ED[0]-EP[0]*EP[0]);
//solve fT
fT_a[ith]=-TK_a[ith]*(EP[0]+ED[0])/(2*(ED[ith]+EP[ith]))/mom[ith];
fT_n[ith]=-TK_n[ith]*(EP[0]+ED[0])/(2*(ED[ith]+EP[ith]))/mom[ith];
}
//write analytic and umeric plot of fP and f0
ofstream out_fP_a("plots/fP_a.xmg"),out_fP_n("plots/fP_n.xmg");
ofstream out_fM_a("plots/fM_a.xmg"),out_fM_n("plots/fM_n.xmg");
ofstream out_f0_a("plots/f0_a.xmg"),out_f0_n("plots/f0_n.xmg");
ofstream out_fT_a("plots/fT_a.xmg"),out_fT_n("plots/fT_n.xmg");
out_fP_a<<"@type xydy"<<endl;
out_fP_n<<"@type xydy"<<endl;
out_f0_a<<"@type xydy"<<endl;
out_f0_n<<"@type xydy"<<endl;
out_fM_a<<"@type xydy"<<endl;
out_fM_n<<"@type xydy"<<endl;
out_fT_a<<"@type xydy"<<endl;
out_fT_n<<"@type xydy"<<endl;
out_f0_a<<Q2[0].med()<<" "<<f0_a[0]<<endl;
out_f0_n<<Q2[0].med()<<" "<<f0_n[0]<<endl;
for(int ith=1;ith<nth;ith++)
{
out_fP_a<<Q2[ith].med()<<" "<<fP_a[ith]<<endl;
out_fP_n<<Q2[ith].med()<<" "<<fP_n[ith]<<endl;
out_fM_a<<Q2[ith].med()<<" "<<fM_a[ith]<<endl;
out_fM_n<<Q2[ith].med()<<" "<<fM_n[ith]<<endl;
out_f0_a<<Q2[ith].med()<<" "<<f0_a[ith]<<endl;
out_f0_n<<Q2[ith].med()<<" "<<f0_n[ith]<<endl;
out_fT_a<<Q2[ith].med()<<" "<<fT_a[ith]<<endl;
out_fT_n<<Q2[ith].med()<<" "<<fT_n[ith]<<endl;
}
ext_EP=EP;
ext_ED=ED;
ext_Q2=Q2;
ext_fP=fP_a;
ext_fM=fM_a;
ext_f0=f0_a;
ext_fT=fT_a;
}
static void Transform(Sha* sha)
{
word32 W[SHA_BLOCK_SIZE / sizeof(word32)];
/* Copy context->state[] to working vars */
word32 a = sha->digest[0];
word32 b = sha->digest[1];
word32 c = sha->digest[2];
word32 d = sha->digest[3];
word32 e = sha->digest[4];
#ifdef USE_SLOW_SHA
word32 t, i;
for (i = 0; i < 16; i++) {
R0(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 20; i++) {
R1(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 40; i++) {
R2(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 60; i++) {
R3(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 80; i++) {
R4(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
#else
/* nearly 1 K bigger in code size but 25% faster */
/* 4 rounds of 20 operations each. Loop unrolled. */
R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
#endif
/* Add the working vars back into digest state[] */
sha->digest[0] += a;
sha->digest[1] += b;
sha->digest[2] += c;
sha->digest[3] += d;
sha->digest[4] += e;
}
static int Transform384(Sha384* sha384)
{
const word64* K = K512;
word32 j;
word64 T[8];
#ifdef CYASSL_SMALL_STACK
word64* W;
W = (word64*) XMALLOC(sizeof(word64) * 16, NULL, DYNAMIC_TYPE_TMP_BUFFER);
if (W == NULL)
return MEMORY_E;
#else
word64 W[16];
#endif
/* Copy digest to working vars */
XMEMCPY(T, sha384->digest, sizeof(T));
#ifdef USE_SLOW_SHA2
/* over twice as small, but 50% slower */
/* 80 operations, not unrolled */
for (j = 0; j < 80; j += 16) {
int m;
for (m = 0; m < 16; m++) { /* braces needed for macros {} */
R2(m);
}
}
#else
/* 80 operations, partially loop unrolled */
for (j = 0; j < 80; j += 16) {
R2( 0); R2( 1); R2( 2); R2( 3);
R2( 4); R2( 5); R2( 6); R2( 7);
R2( 8); R2( 9); R2(10); R2(11);
R2(12); R2(13); R2(14); R2(15);
}
#endif /* USE_SLOW_SHA2 */
/* Add the working vars back into digest */
sha384->digest[0] += a(0);
sha384->digest[1] += b(0);
sha384->digest[2] += c(0);
sha384->digest[3] += d(0);
sha384->digest[4] += e(0);
sha384->digest[5] += f(0);
sha384->digest[6] += g(0);
sha384->digest[7] += h(0);
/* Wipe variables */
XMEMSET(W, 0, sizeof(word64) * 16);
XMEMSET(T, 0, sizeof(T));
#ifdef CYASSL_SMALL_STACK
XFREE(W, NULL, DYNAMIC_TYPE_TMP_BUFFER);
#endif
return 0;
}
bool b2LineJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 c1 = b1->m_sweep.c;
float32 a1 = b1->m_sweep.a;
b2Vec2 c2 = b2->m_sweep.c;
float32 a2 = b2->m_sweep.a;
// Solve linear limit constraint.
float32 linearError = 0.0f, angularError = 0.0f;
bool active = false;
float32 C2 = 0.0f;
b2Mat22 R1(a1), R2(a2);
b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
b2Vec2 d = c2 + r2 - c1 - r1;
if (m_enableLimit)
{
m_axis = b2Mul(R1, m_localXAxis1);
m_a1 = b2Cross(d + r1, m_axis);
m_a2 = b2Cross(r2, m_axis);
float32 translation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
linearError = b2Abs(translation);
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
linearError = m_lowerTranslation - translation;
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
linearError = translation - m_upperTranslation;
active = true;
}
}
m_perp = b2Mul(R1, m_localYAxis1);
m_s1 = b2Cross(d + r1, m_perp);
m_s2 = b2Cross(r2, m_perp);
b2Vec2 impulse;
float32 C1;
C1 = b2Dot(m_perp, d);
linearError = b2Max(linearError, b2Abs(C1));
angularError = 0.0f;
if (active)
{
float32 m1 = m_invMassA, m2 = m_invMassB;
float32 i1 = m_invIA, i2 = m_invIB;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
m_K.col1.Set(k11, k12);
m_K.col2.Set(k12, k22);
b2Vec2 C;
C.x = C1;
C.y = C2;
impulse = m_K.Solve(-C);
}
else
{
float32 m1 = m_invMassA, m2 = m_invMassB;
float32 i1 = m_invIA, i2 = m_invIB;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 impulse1;
if (k11 != 0.0f)
{
impulse1 = - C1 / k11;
}
else
{
impulse1 = 0.0f;
}
impulse.x = impulse1;
impulse.y = 0.0f;
}
b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;
float32 L1 = impulse.x * m_s1 + impulse.y * m_a1;
float32 L2 = impulse.x * m_s2 + impulse.y * m_a2;
c1 -= m_invMassA * P;
a1 -= m_invIA * L1;
c2 += m_invMassB * P;
a2 += m_invIB * L2;
// TODO_ERIN remove need for this.
b1->m_sweep.c = c1;
b1->m_sweep.a = a1;
b2->m_sweep.c = c2;
b2->m_sweep.a = a2;
b1->SynchronizeTransform();
b2->SynchronizeTransform();
return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
