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; }