uint32_t MtfCoordinateConverter::getLayerNumber(uint32_t rawId){
uint32_t aLayer = 0;
DetId detId(rawId);
if (detId.det() != DetId::Muon) std::cout << "PROBLEM: hit in unknown Det, detID: "<<detId.det()<<std::endl;
switch (detId.subdetId()) {
case MuonSubdetId::RPC: {
RPCDetIdUtil aIdUtil(rawId);
aLayer = aIdUtil.layer() + 10*(!aIdUtil.isBarrel());
}
break;
case MuonSubdetId::DT: {
DTChamberId dt(rawId);
aLayer = dt.station();
break;
}
case MuonSubdetId::CSC: {
CSCDetId csc(rawId);
aLayer = csc.station();
break;
}
}
return aLayer;
}
BOOL COpenFileDlg::FillFileList()
{
CUStringConvert strCnv;
// clear list
m_ctrlRequestedFiles.DeleteAllItems();
CFileFind fileFind;
if ((m_strDir[m_strDir.GetLength()-1]=='\\') )
m_strDir=m_strDir.Left(m_strDir.GetLength()-1);
// If directory does not exist, do not waste time
if (!fileFind.FindFile( strCnv.ToT( m_strDir )) && (m_strDir.GetLength()>3) )
{
m_strDir=g_config.GetAppPath();
}
// create the extions strings
MakeExtString();
// do a recursive search for all the files with the proper extention
AddRecursiveFiles( m_strDir, 0 );
CSortClass csc( &m_ctrlRequestedFiles, m_nSortedColumn );
ReSortColumns( );
// Select All Items
m_ctrlRequestedFiles.SelectAll();
return TRUE;
}
/*
Traverse and sort the columns, the sort order is in the internal array
use bSetSort as true for a call from outside the class
*/
void CMultiColumnSortListCtrl::SortCombinedColumns(bool bSetSort /*= false*/)
{
if( bSetSort )
{
m_bSorting = true;
}
int iNumCombinedSortedCols = GetNumCombinedSortedColumns();
for( int nIndex = iNumCombinedSortedCols - 1; nIndex >= 0 ; nIndex-- )
{
SORT_STATE ssEachItem = GetItemSortState( m_aCombinedSortedColumns[nIndex] );
SORT_TYPE sortType = GetColumnType(m_aCombinedSortedColumns[nIndex]);
CSortClass csc( this, m_aCombinedSortedColumns[nIndex], sortType );
csc.Sort( DESCENDING == ssEachItem ); //Ariel Benary <*****@*****.**>
//refresh the sort image
m_ctlHeaderCtrl.SetSortImage( m_aCombinedSortedColumns[nIndex], ssEachItem );
}
if( bSetSort )
{
m_bSorting = false;
}
}
csc
Ran::rand(bir ssr) const
{
char bufI[maxI];
size_t sizeI = 0;
if (m_subs.size() == 2) {
const csc bufI0 = dynamic_cast<Base*>(hits[0])->rand(ssr);
const csc bufI1 = dynamic_cast<Base*>(hits[1])->rand(ssr);
const size_t lenI0 = bufI0.size();
const size_t lenI1 = bufI1.size();
if (lenI0 == lenI1) { /* if ssr alternatives are of length */
switch (lenI0) {
case 1:
{
const char val = (bufI0[0] < bufI1[0]) ?
char_rangerand(bufI0[0], bufI1[0]) :
char_rangerand(bufI1[0], bufI0[0]);
memcpy(bufI, &val, lenI0);
sizeI = lenI0;
break;
}
default:
std::cerr << "Cannot handle case for sizeI == " << lenI0 << std::endl; break;
}
} else { PERR("Cannot handle case for lenI0 != lenI1. \todo How should we interpolate ranges of different lengths???\n"); }
} else {
std::cerr << "Cannot handle case for elmsN=" << hits.size() << std::endl;
}
return csc(bufI, sizeI);
}
RCP<const Basic> Csc::subs(const map_basic_basic &subs_dict) const
{
RCP<const Csc> self = rcp_const_cast<Csc>(rcp(this));
auto it = subs_dict.find(self);
if (it != subs_dict.end())
return it->second;
RCP<const Basic> arg = arg_->subs(subs_dict);
if (arg == arg_)
return self;
else
return csc(arg);
}
int main(int argc, char **argv)
{
static const unsigned colorspaces[] = {
0,
V4L2_COLORSPACE_SMPTE170M,
V4L2_COLORSPACE_SMPTE240M,
V4L2_COLORSPACE_REC709,
0,
V4L2_COLORSPACE_470_SYSTEM_M,
V4L2_COLORSPACE_470_SYSTEM_BG,
0,
V4L2_COLORSPACE_SRGB,
};
static const char * const colorspace_names[] = {
"",
"V4L2_COLORSPACE_SMPTE170M",
"V4L2_COLORSPACE_SMPTE240M",
"V4L2_COLORSPACE_REC709",
"",
"V4L2_COLORSPACE_470_SYSTEM_M",
"V4L2_COLORSPACE_470_SYSTEM_BG",
"",
"V4L2_COLORSPACE_SRGB",
};
int i;
int c;
printf("/* Generated table */\n");
printf("const struct color16 tpg_csc_colors[V4L2_COLORSPACE_SRGB + 1][TPG_COLOR_CSC_BLACK + 1] = {\n");
for (c = 0; c <= V4L2_COLORSPACE_SRGB; c++) {
for (i = 0; i <= TPG_COLOR_CSC_BLACK; i++) {
double r, g, b;
if (colorspaces[c] == 0)
continue;
r = tpg_colors[i].r / 255.0;
g = tpg_colors[i].g / 255.0;
b = tpg_colors[i].b / 255.0;
csc(c, &r, &g, &b);
printf("\t[%s][%d] = { %d, %d, %d },\n", colorspace_names[c], i,
(int)(r * 4080), (int)(g * 4080), (int)(b * 4080));
}
}
printf("};\n\n");
return 0;
}
void COpenFileDlg::ReSortColumns( )
{
CSortClass csc(&m_ctrlRequestedFiles, m_nSortedColumn);
CSortClass::EDataType eSortType( CSortClass::dtSTRING );
switch ( m_nSortedColumn )
{
case FILE_OPEN_NAME: eSortType = CSortClass::dtSTRING; break;
case FILE_OPEN_TYPE: eSortType = CSortClass::dtSTRING; break;
case FILE_OPEN_DATE: eSortType = CSortClass::dtDATETIME; break;
case FILE_OPEN_PATH: eSortType = CSortClass::dtSTRING; break;
case FILE_OPEN_SIZE: eSortType = CSortClass::dtINT; break;
default:
ASSERT( FALSE );
}
csc.Sort( m_bSortAscending, eSortType );
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "robotcontrol");
ros::start();
// Init Robulab and MocapRobulab class
Robulab10 Robot;
MoCapMessenger MocapRobulab;
MoCapMessenger MocapGoalLeft;
MoCapMessenger MocapGoalRight;
// To receive data from MocapRobulabRobulab
MocapRobulab.sub = MocapRobulab.n.subscribe("/evart/robulabhalf/PO", 2000, &MoCapMessenger::callbackFunction, &MocapRobulab);
MocapGoalLeft.sub = MocapGoalLeft.n.subscribe("/evart/goal_left/PO", 2000, &MoCapMessenger::callbackFunction, &MocapGoalLeft);
MocapGoalRight.sub = MocapGoalRight.n.subscribe("/evart/goal_right/PO", 2000, &MoCapMessenger::callbackFunction, &MocapGoalRight);
// Init ROS
ros::NodeHandle node_message;
// To receive data from vision control
ros::Subscriber _subscriber = node_message.subscribe("datacircles", 2000, &getDataCircles);
// To send data to Matlab
ros::Publisher _publisherrobotcontrol = node_message.advertise<std_msgs::Float64MultiArray>("VelocityControl", 2000);
ros::Publisher _MocapRobulabPublisher = node_message.advertise<std_msgs::Float64MultiArray>("RobotState", 2000);
ros::Publisher _MocapGoalPublisher = node_message.advertise<std_msgs::Float64MultiArray>("GoalState", 2000);
// Open the connection with the Robot Robulab
Robot.establish_connection();
// Active signal (CTRL + C interrupt)
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = my_handler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
/// Elliptical Coordinates Control Feedback
double x1, y1, x2, y2, A, B, C, xi, eta, betae;
double x1centered, y1centered, x2centered, y2centered;
double x1centered_before, y1centered_before, x2centered_before, y2centered_before;
double omega, omegao;
double v, w;
bool goal_reached = false;
int iter=0;
v = 0; w = 0;
/// ROS data message
std::vector<double> robotcontrol_data_t;
std::vector<std::vector<double> > robotcontol_data;
std::vector<double> robotstate_t;
std::vector<std::vector<double> > robotstate_data;
std::vector<double> goalstate_t;
std::vector<double> support_variable;
std::vector<std::vector<double> > goalstate_data;
std::vector<double> support_vector_position_switch_control;
/// Parameter of the camera (defined by calibration)
double alphax = 712.27744;
double alphay = 711.92580;
double PPx = 330.98367; // Principal point X
double PPy = 273.41515; // Principal point Y
double a = 70/2;
double kc[5] = {-0.39852, 0.24318, 0.00180, -0.00011, 0.00000};
double pixelxy[2];
double cc[2] = {PPx, PPy};
double fc[2] = {alphax, alphay};
double alpha_c = 0;
double xp[2];
double x1p, y1p;
double K0, K, K1, K2;
double tau, alpha, betap, beta_d, s;
double p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11, p12, p13;
/// Gains of the control
K0 = atof(argv[2]);
K = atof(argv[3]);
K1 = atof(argv[4]);
K2 = atof(argv[5]);
/// Delay before to get information from MocapRobulab (to avoid null data)
std::cout << "... Initilization Virtual Tracking..." << std::endl;
int samples = 0.9/0.015; // 1 second divided by 20ms
double Ka[samples];
double x1centereddist, x2centereddist, y1centereddist,y2centereddist;
for (int i=1; i<=samples; ++i){
Ka[i] = (double)i/samples;
}
std::cout << "Parameters initialized... " << std::endl;
/// Starting to call the communication
double timenowdouble = time(NULL)+1;
while(time(NULL)<timenowdouble){
ros::spinOnce();
ros::Duration(0.02).sleep();
}
bool target_lost = false;
bool go_though_door = false;
double iter_error = 0;
iter=1;
/// TRACK CONTROL
while(ros::ok() && goal_reached == false && stop_interrupt==false ){
ros::spinOnce();
/// Take data from MocapRobulab, tracking the Robot
robotstate_t.clear();
robotstate_t = MocapRobulab.item_XY_YAW_configuration_OFFSET(-2.23393);
robotstate_data.push_back(robotstate_t);
goalstate_t.clear();
support_variable.clear();
goalstate_t = MocapGoalLeft.item_XY_YAW_configuration();
support_variable = MocapGoalRight.item_XY_YAW_configuration();
/// Concatenate vectors such that the vector is [x1 y1 t1 x2 y2 t2]
goalstate_t.insert( goalstate_t.end(), support_variable.begin(), support_variable.end() );
goalstate_data.push_back(goalstate_t);
std::cout << "Object: x " << goalstate_t[0] << " y " << goalstate_t[1] << "th " << goalstate_t[2] << std::endl;
std::cout << "Object: x " << goalstate_t[3] << " y " << goalstate_t[4] << "th " << goalstate_t[5] << std::endl;
/// Elliptic coordinates computed from the image plane measurements
/// Input parser...(feature points)
target_lost = false;
/// SECURITY CHECK
if(CirclesData[0][0]==CirclesData[1][0])
{
target_lost = true;
std::cout << "Lost one "<< std::endl;
}
if(CirclesData[0][0]>CirclesData[1][0])
{
x1centered = CirclesData[1][0] - PPx;
y1centered = CirclesData[1][1] - PPy;
x2centered = CirclesData[0][0] - PPx;
y2centered = CirclesData[0][1] - PPy;
}
else{
x1centered = CirclesData[0][0] - PPx;
y1centered = CirclesData[0][1] - PPy;
x2centered = CirclesData[1][0] - PPx;
y2centered = CirclesData[1][1] - PPy;
}
/*
if(CirclesData[0][0]>CirclesData[1][0])
{
x1centered = CirclesData[1][0] - PPx;
y1centered = CirclesData[1][1] - PPy;
x2centered = CirclesData[0][0] - PPx;
y2centered = CirclesData[0][1] - PPy;
}
else{
x1centered = CirclesData[0][0] - PPx;
y1centered = CirclesData[0][1] - PPy;
x2centered = CirclesData[1][0] - PPx;
y2centered = CirclesData[1][1] - PPy;
}
*/
/*
if(CirclesData[0][0]>CirclesData[1][0])
{
x1centereddist = CirclesData[1][0] - PPx;
y1centereddist = CirclesData[1][1] - PPy;
x2centereddist = CirclesData[0][0] - PPx;
y2centereddist = CirclesData[0][1] - PPy;
}
else{
x1centereddist = CirclesData[0][0] - PPx;
y1centereddist = CirclesData[0][1] - PPy;
x2centereddist = CirclesData[1][0] - PPx;
y2centereddist = CirclesData[1][1] - PPy;
}
//void remove_distorsion(double *pixelxy, double *cc, double *fc, double *kc, double alpha_c, double *xp)
pixelxy[0]=x1centereddist ;
pixelxy[1]=y1centereddist ;
remove_distorsion(pixelxy, cc, fc, kc, alpha_c, xp);
x1centered = xp[0];
y1centered = xp[1];
pixelxy[0]=x2centereddist ;
pixelxy[1]=y2centereddist ;
remove_distorsion(pixelxy, cc, fc, kc, alpha_c, xp);
x2centered = xp[0];
y2centered = xp[1];
*/
/// If the targets or just one are not detected
if((x1centered==x1centered_before && y1centered==y1centered_before) || (x2centered==x2centered_before && y2centered==y2centered_before))
{
std::cout << "Equal data" << std::endl;
//target_lost = true;
}
if((iter % 200)==0){
x1centered_before = x1centered;
x2centered_before = x2centered;
y1centered_before = y1centered;
y2centered_before = y2centered;
}
/// If the two target are not in the same level (one of them is noise)
if(fabs(y1centered-y2centered)>30)
{
target_lost = true;
std::cout << "Different LEVEL" << std::endl;
}
x1 = x1centered;
y1 = (y1centered/fabs(y1centered))*y1centered;
x2 = x2centered;
y2 = (y2centered/fabs(y2centered))*y2centered;
// Check ellipses position
std::cout << " ---------------------------------- " << std::endl;
std::cout << " x1c = " << x1centered << " x1cb " << x1centered_before << std::endl;
std::cout << " x2c = " << x2centered << " x2cb " << x2centered_before << std::endl;
std::cout << " y1c = " << y1centered << " y1cb " << y1centered_before << std::endl;
std::cout << " y2c = " << y2centered << " y2cb " << y2centered_before << std::endl;
std::cout << " ---------------------------------- " << std::endl;
//std::cout << "a x " << alphax << std::endl;
/// CONTROL LAW
/// Bipolar coordinates in the image plane...
A = y2*sqrt(x1*x1+alphax*alphax);
B = y1*sqrt(x2*x2+alphax*alphax);
C = sqrt((x1*y2-x2*y1)*(x1*y2-x2*y1)+alphax*alphax*(y2-y1)*(y2-y1));
//std::cout << "A " << A << "\n B " << B << "\n C" << C << std::endl;
xi = acosh((A+B)/C);
eta = M_PI/2-acos((A-B)/C);
betae = -(atan(x1/alphax)+(atan(x2/alphax)-atan(x1/alphax))/2);
//std::cout << "xi " << xi << "\n eta " << eta << "\n betae" << betae << std::endl;
tau = log(A/B);
alpha = M_PI-fabs(atan(x1/alphax)-atan(x2/alphax));
betap = atan(sin(alpha)*sinh(tau)/(1-cos(alpha)*cosh(tau)))-(atan(tan(eta)*tanh(xi))-betae+M_PI)+M_PI;
//std::cout << "tau " << tau << "\n alpha " << alpha << "\n betap" << betap << std::endl;
beta_d = -atan2(K*sinh(tau),sin(alpha));
s = betap-beta_d;
v = K2*(alpha*cos(betap)-tau*sin(betap));
p1 = K1 * s;
p2 = cos(alpha)-cosh(tau);
p3 = cos(alpha)*cosh(tau);
p4 = cot(alpha)*coth(tau);
p5 = csc(alpha)*csch(tau);
p6 = cos(betap)*csc(alpha);
p7 = csch(tau)*sin(betap);
p8 = cos(alpha)+cosh(tau);
p9 = cosh(tau)*sin(betap);
p10 = cos(betap)*cot(alpha)*sinh(tau);
p11 = K;
p12 = csc(alpha);
p13 = sinh(tau);
if (tau <= 0.0001)
omega = K0*(K1 * s + v * (1/a * (1 + K + cos(alpha)) * cot(0.5 * alpha) * sin(betap)));
else{
omega = K0 * (p1 + v * (-1/a * 1/p2 * p3*p3 * (1+1/sqrt(pow((p4+p5),2))) * (p6+p7) + K * p8 * csc(alpha) * (p9+p10) * pow((a + a*p11*p11*p12*p12*p13*p13),-1) ));
}
if (v>1.5){
Robot.move_robot(0,0);
std::cout << "OHHHH DOVE VAIII " << v << std::endl;
exit(-1);
}
w = omega;
// Some print out
std::cout << " ----------------------------------------- " << std::endl;
std::cout << "tau " << tau << std::endl;
std::cout << "alpha: " << alpha << std::endl;
std::cout << "betap: " << betap << std::endl;
std::cout << "beta_d: " << beta_d << std::endl;
std::cout << "s: " << s << std::endl;
std::cout << "Gain: K0 " << argv[2] << " ; K " << argv[3] << " ; K1 " << argv[4] << " K2 " << argv[5] << std::endl;
std::cout << "Robot controls - v: " << v << " omega: " << w << " omegao: " << omegao << std::endl;
//std::cout << " ----------------------------------------- " << std::endl;
/// Move ROBOT
if((CirclesData[1][1]<20 || CirclesData[0][1]<20) || (CirclesData[1][1]>470 || CirclesData[0][1]>470)){
Robot.move_robot(0, 0);
if(CirclesData[0][0]!=0 && CirclesData[0][1]!=0)
stop_interrupt=true;
go_though_door=true;
}
else
if(!target_lost){
if( atof(argv[1])==1 || atof(argv[1])==2){
std::cout << "Move!" << std::endl;
Robot.move_robot(v, w);
}
}
else
{
iter_error++;
std::cout << "Error detected. " << iter_error << std::endl;
}
if(iter_error>100){
stop_interrupt=true;
exit(-1);
std::cout << "TARGET NOT VALID" << std::endl;
Robot.move_robot(0,0);
}
/// Create message to send to Matlab
robotcontrol_data_t.push_back(v);
robotcontrol_data_t.push_back(w);
robotcontol_data.push_back(robotcontrol_data_t);
robotcontrol_data_t.clear();
support_vector_position_switch_control.clear();
support_vector_position_switch_control = MocapRobulab.item_XY_YAW_configuration_OFFSET(-2.23393);
ros::Duration(0.02).sleep();
++iter;
}
double spaceoffset = 0.9;
double timeoffset = spaceoffset / v;
std::cout << "last pos change control " << support_vector_position_switch_control[0] << " " << support_vector_position_switch_control[1] << std::endl;
if(go_though_door && atof(argv[1])==2){
stop_interrupt=false;
std::cout << "Through the door" << std::endl;
timenowdouble = time(NULL)+timeoffset;
while(time(NULL)<timenowdouble && stop_interrupt==false){
ros::spinOnce();
Robot.move_robot(v,0);
/// Take data from MocapRobulab, tracking the Robot
robotstate_t.clear();
robotstate_t = MocapRobulab.item_XY_YAW_configuration_OFFSET(-2.23393);
robotstate_data.push_back(robotstate_t);
robotcontrol_data_t.clear();
robotcontrol_data_t.push_back(v);
robotcontrol_data_t.push_back(0);
robotcontol_data.push_back(robotcontrol_data_t);
/// Concatenate vectors such that the vector is [x1 y1 t1 x2 y2 t2]
goalstate_t.insert( goalstate_t.end(), support_variable.begin(), support_variable.end() );
goalstate_data.push_back(goalstate_t);
ros::Duration(0.02).sleep();
}
}
Robot.move_robot(0, 0);
/*
timenowdouble = time(NULL)+2;
while(time(NULL)<timenowdouble && stop_interrupt==false){
Robot.move_robot(0.3, 0);
ros::Duration(0.01).sleep();
}
*/
// Save switch control pos value as last value
robotstate_data.push_back(support_vector_position_switch_control);
goalstate_t.clear();
goalstate_t.push_back(K0);
goalstate_t.push_back(K);
goalstate_t.push_back(-1);
goalstate_t.push_back(K1);
goalstate_t.push_back(K2);
goalstate_t.push_back(-1);
goalstate_data.push_back(goalstate_t);
std::cout << "Convertingg Data RobotControl to Matlab" << std::endl;
Eigen::MatrixXd robotcontrol_data_matrix = Eigen::MatrixXd::Zero(robotcontol_data.size(),2);
std_msgs::Float64MultiArray robotcontrol_data_msg;
for(unsigned int c=0; c<robotcontol_data.size(); ++c){
robotcontrol_data_matrix(c,0) = robotcontol_data[c][0];
robotcontrol_data_matrix(c,1) = robotcontol_data[c][1];
}
std::cout << "Converting Data RobotState to Matlab" << std::endl;
Eigen::MatrixXd robotstate_data_matrix = Eigen::MatrixXd::Zero(robotstate_data.size(),3);
std_msgs::Float64MultiArray robotstate_data_msg;
for(unsigned int c=0; c<robotstate_data.size(); ++c){
robotstate_data_matrix(c,0) = robotstate_data[c][0];
robotstate_data_matrix(c,1) = robotstate_data[c][1];
robotstate_data_matrix(c,2) = robotstate_data[c][2];
}
std::cout << "Converting Data GoalState to Matlab" << std::endl;
Eigen::MatrixXd goalstate_data_matrix = Eigen::MatrixXd::Zero(goalstate_data.size(),6);
std_msgs::Float64MultiArray goalstate_data_msg;
for(unsigned int c=0; c<goalstate_data.size(); ++c){
goalstate_data_matrix(c,0) = goalstate_data[c][0];
goalstate_data_matrix(c,1) = goalstate_data[c][1];
goalstate_data_matrix(c,2) = goalstate_data[c][2];
goalstate_data_matrix(c,3) = goalstate_data[c][3];
goalstate_data_matrix(c,4) = goalstate_data[c][4];
goalstate_data_matrix(c,5) = goalstate_data[c][5];
}
/// Define the message to send
tf::matrixEigenToMsg(robotcontrol_data_matrix, robotcontrol_data_msg);
tf::matrixEigenToMsg(robotstate_data_matrix, robotstate_data_msg);
tf::matrixEigenToMsg(goalstate_data_matrix, goalstate_data_msg);
std::cout << "Done. " << std::endl;
for(unsigned int it=0; it<300; it++){
_publisherrobotcontrol.publish(robotcontrol_data_msg);
_MocapRobulabPublisher.publish(robotstate_data_msg);
_MocapGoalPublisher.publish(goalstate_data_msg);
ros::Duration(0.01).sleep();
}
Robot.move_robot(0, 0);
return 0;
}
RCP<const Basic> Csc::diff(const RCP<const Symbol> &x) const
{
return mul(mul(mul(cot(arg_), csc(arg_)), minus_one), arg_->diff(x));
}
static RCP<const Basic> diff(const Csc &self,
const RCP<const Symbol> &x) {
return mul(mul(mul(cot(self.get_arg()), csc(self.get_arg())),
minus_one), self.get_arg()->diff(x));
}
File* load_to(const char * pathR) {
// WARNING: Only works if boost::cref is *NOT* copy constructed in
// File* relative_load_to(const csc& pathR)
return load_to(csc(pathR// boost::cref(pathR)
));
}
void StrandBlockSolver::lhsDissipation()
{
int jj=nPstr+2,nn=nFaces+nBedges;
Array4D<double> aa(nq,nq,jj,nn);
for (int n=0; n<nFaces+nBedges; n++)
for (int j=0; j<nPstr+2; j++)
for (int k=0; k<nq; k++)
for (int l=0; l<nq; l++) aa(l,k,j,n) = 0.;
// unstructured faces
int c1,c2,n1,n2,jp,fc,m,mm,m1l,m1u,m2l,m2u,npts=1;
double a[nq*nq];
for (int n=0; n<nEdges; n++){
c1 = edge(0,n);
c2 = edge(1,n);
m1l = ncsc(c1 );
m1u = ncsc(c1+1);
m2l = ncsc(c2 );
m2u = ncsc(c2+1);
fc = fClip(c1);
if (fClip(c2) > fc) fc = fClip(c2);
for (int j=1; j<fc+1; j++){
sys->lhsDisFluxJacobian(npts,&facs(0,j,n),&xvs(j,n),
&q(0,j,c1),&q(0,j,c2),&a[0]);
for (int k=0; k<nq*nq; k++) a[k] *= .5;
// diagonal contributions
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++){
dd(l,k,j,c1) += a[m+l];
dd(l,k,j,c2) += a[m+l];
}}
// off-diagonal contributions
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++){
aa(l,k,j,c1) = a[m+l];
aa(l,k,j,c2) = a[m+l];
}}
for (int mm=m1l; mm<m1u; mm++){
nn = csc(mm);
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++)
bu(l,k,j,mm) -= aa(l,k,j,nn);
}}
for (int mm=m2l; mm<m2u; mm++){
nn = csc(mm);
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++)
bu(l,k,j,mm) -= aa(l,k,j,nn);
}}
// reset working array to zero
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++){
aa(l,k,j,c1) = 0.;
aa(l,k,j,c2) = 0.;
}}
}}
aa.deallocate();
// structured faces
double Ax,Ay,ds,eps=1.e-14;
for (int n=0; n<nFaces-nGfaces; n++){
n1 = face(0,n);
n2 = face(1,n);
for (int j=0; j<fClip(n)+1; j++){
jp = j+1;
Ax = facu(0,j,n);
Ay = facu(1,j,n);
ds = Ax*Ax+Ay*Ay;
if (fabs(ds) < eps){
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++)
a[m+l] = 0.;
}}
else
sys->lhsDisFluxJacobian(npts,&facu(0,j,n),&xvu(j,n),
&q(0,j,n),&q(0,jp,n),&a[0]);
for (int k=0; k<nq; k++){
m = k*nq;
for (int l=0; l<nq; l++){
dd(l,k,j ,n) += (a[m+l]*.5);
dp(l,k,j ,n) -= (a[m+l]*.5);
dd(l,k,jp,n) += (a[m+l]*.5);
dm(l,k,jp,n) -= (a[m+l]*.5);
}}}}
}
QNFA* sequence(const QChar *d, int length, QNFA **end, bool cs)
{
QNFA *nfa, *set = 0, *prev = 0, *first = 0;
for ( int i = 0; i < length; ++i )
{
QChar c = d[i];
if ( c == QLatin1Char('\\') )
{
c = d[++i];
if ( c == QLatin1Char('n') )
{
c = '\n';
} else if ( c == QLatin1Char('t') ) {
c = '\t';
} else if ( c == QLatin1Char('r') ) {
c = '\r';
}
if ( set )
{
set->c << c.unicode();
} else {
nfa = new QNFA;
nfa->c << c.unicode();
if ( prev )
prev->out.next = nfa;
prev = nfa;
}
} else if ( c == QLatin1Char('$') ) {
// char classes
c = d[++i];
if ( set )
{
if ( c == QLatin1Char('s') )
set->assertion |= Space;
else if ( c == QLatin1Char('S') )
set->assertion |= NonSpace;
else if ( c == QLatin1Char('d') )
set->assertion |= Digit;
else if ( c == QLatin1Char('D') )
set->assertion |= NonDigit;
else if ( c == QLatin1Char('w') )
set->assertion |= Word;
else if ( c == QLatin1Char('W') )
set->assertion |= NonWord;
else
set->c << QLatin1Char('$').unicode() << c.unicode();
} else {
nfa = new QNFA;
if ( c == QLatin1Char('s') )
nfa->assertion |= Space;
else if ( c == QLatin1Char('S') )
nfa->assertion |= NonSpace;
else if ( c == QLatin1Char('d') )
nfa->assertion |= Digit;
else if ( c == QLatin1Char('D') )
nfa->assertion |= NonDigit;
else if ( c == QLatin1Char('w') )
nfa->assertion |= Word;
else if ( c == QLatin1Char('W') )
nfa->assertion |= NonWord;
else {
nfa->c << QLatin1Char('$').unicode();
--i;
}
if ( prev )
prev->out.next = nfa;
prev = nfa;
}
} else if ( c == QLatin1Char('[') ) {
if ( set )
{
set->c << c.unicode();
// qWarning("Nested sets are not supported (and useless BTW)...");
continue;
}
// enter set...
set = new QNFA;
//qDebug("set start");
} else if ( c == QLatin1Char(']') ) {
if ( !set )
{
qWarning("Unmatched set closing marker");
continue;
}
// leave set...
if ( prev )
prev->out.next = set;
prev = set;
set = 0;
//qDebug("set end");
/*
} else if ( c == QLatin1Char('(') ) {
// allow trivial groups
QList<int> cuts;
int idx = i, nest = 1;
while ( nest && (++idx < length) )
{
if ( d[idx] == '\\' )
{
++idx;
continue;
} else if ( d[idx] == '(' ) {
++nest;
} else if ( d[idx] == ')' ) {
--nest;
} else if ( (nest == 1) && (d[idx] == '|') ) {
cuts << idx;
} else if ( d[idx] == '[' ) {
while ( ++idx < length )
{
if ( d[idx] == '\\' )
{
++idx;
continue;
} else if ( d[idx] == ']' ) {
break;
}
}
}
}
*/
} else if ( set ) {
if ( (c == QLatin1Char('^')) && !set->c.count() )
{
set->c << '\0';
continue;
}
quint16 prev = set->c.count() ? set->c.at(set->c.length() - 1) : '\0';
if ( (c == '-') && (prev != '\0') && ((i + 1) < length) )
{
quint16 cse = d[++i].unicode();
for ( quint16 csi = prev + 1; csi <= cse; ++csi )
{
QChar csc(csi);
if ( c.isLetter() && !cs )
set->c << c.toLower().unicode() << c.toUpper().unicode();
else
set->c << csi;
}
} else {
if ( c.isLetter() && !cs )
set->c << c.toLower().unicode() << c.toUpper().unicode();
else
set->c << c.unicode();
}
//qDebug("set << %c", c.toLatin1());
} else if ( c == QLatin1Char('+') ) {
if ( prev ) prev->assertion |= OneOrMore;
} else if ( c == QLatin1Char('*') ) {
if ( prev ) prev->assertion |= ZeroOrMore;
} else if ( c == QLatin1Char('?') ) {
if ( prev ) prev->assertion |= ZeroOrOne;
} else {
nfa = new QNFA;
if ( c.isLetter() && !cs )
{
nfa->c << c.toLower().unicode() << c.toUpper().unicode();
} else {
nfa->c << c.unicode();
}
if ( prev )
prev->out.next = nfa;
prev = nfa;
}
if ( !first )
first = prev;
}
if ( end )
{
*end = prev;
}
return first;
}
