/* Subroutine */ int hcore_(doublereal *coord, doublereal *h__, doublereal *w,
doublereal *wj, doublereal *wk, doublereal *enuclr)
{
/* Initialized data */
static integer icalcn = 0;
/* Format strings */
static char fmt_120[] = "(10f8.4)";
/* System generated locals */
integer i__1, i__2, i__3, i__4;
/* Builtin functions */
integer i_indx(char *, char *, ftnlen, ftnlen);
/* Subroutine */ int s_copy(char *, char *, ftnlen, ftnlen);
integer s_wsfe(cilist *), do_fio(integer *, char *, ftnlen), e_wsfe(void);
/* Local variables */
static integer i__, j, i1, i2, j1, j2, ia, ib, ic;
static doublereal di[81] /* was [9][9] */;
static integer ja, jb, jc, ii, jj, ni, nj, kr;
static doublereal xf, yf, zf, e1b[10], e2a[10];
static integer im1, io1, jo1;
static doublereal wjd[100], wkd[100];
static integer kro;
static doublereal half;
static integer ione;
static doublereal fnuc, enuc;
extern doublereal reada_(char *, integer *, ftnlen);
static logical debug, fldon, first;
extern /* Subroutine */ int h1elec_(integer *, integer *, doublereal *,
doublereal *, doublereal *), addhcr_(doublereal *), addnuc_(
doublereal *);
static doublereal fldcon, hterme, cutoff;
extern /* Subroutine */ int rotate_(integer *, integer *, doublereal *,
doublereal *, doublereal *, integer *, doublereal *, doublereal *,
doublereal *, doublereal *), vecprt_(doublereal *, integer *);
static char tmpkey[241];
extern /* Subroutine */ int solrot_(integer *, integer *, doublereal *,
doublereal *, doublereal *, doublereal *, integer *, doublereal *,
doublereal *, doublereal *, doublereal *);
/* Fortran I/O blocks */
static cilist io___11 = { 0, 6, 0, "(/10X,'THE ELECTRIC FIELD IS',3F10.5)"
, 0 };
static cilist io___12 = { 0, 6, 0, "(10X,'IN 8*A.U. (8*27.21/0.529 VOLTS"
"/ANGSTROM)',/)", 0 };
static cilist io___44 = { 0, 6, 0, "(//10X,'ONE-ELECTRON MATRIX FROM HCO"
"RE')", 0 };
static cilist io___45 = { 0, 6, 0, "(//10X,'TWO-ELECTRON MATRIX IN HCORE"
"'/)", 0 };
static cilist io___46 = { 0, 6, 0, fmt_120, 0 };
static cilist io___47 = { 0, 6, 0, "(//10X,'TWO-ELECTRON J MATRIX IN HCO"
"RE'/)", 0 };
static cilist io___48 = { 0, 6, 0, fmt_120, 0 };
static cilist io___49 = { 0, 6, 0, "(//10X,'TWO-ELECTRON K MATRIX IN HCO"
"RE'/)", 0 };
static cilist io___50 = { 0, 6, 0, fmt_120, 0 };
/* COMDECK SIZES */
/* *********************************************************************** */
/* THIS FILE CONTAINS ALL THE ARRAY SIZES FOR USE IN MOPAC. */
/* THERE ARE ONLY 5 PARAMETERS THAT THE PROGRAMMER NEED SET: */
/* MAXHEV = MAXIMUM NUMBER OF HEAVY ATOMS (HEAVY: NON-HYDROGEN ATOMS) */
/* MAXLIT = MAXIMUM NUMBER OF HYDROGEN ATOMS. */
/* MAXTIM = DEFAULT TIME FOR A JOB. (SECONDS) */
/* MAXDMP = DEFAULT TIME FOR AUTOMATIC RESTART FILE GENERATION (SECS) */
/* ISYBYL = 1 IF MOPAC IS TO BE USED IN THE SYBYL PACKAGE, =0 OTHERWISE */
/* SEE ALSO NMECI, NPULAY AND MESP AT THE END OF THIS FILE */
/* *********************************************************************** */
/* THE FOLLOWING CODE DOES NOT NEED TO BE ALTERED BY THE PROGRAMMER */
/* *********************************************************************** */
/* ALL OTHER PARAMETERS ARE DERIVED FUNCTIONS OF THESE TWO PARAMETERS */
/* NAME DEFINITION */
/* NUMATM MAXIMUM NUMBER OF ATOMS ALLOWED. */
/* MAXORB MAXIMUM NUMBER OF ORBITALS ALLOWED. */
/* MAXPAR MAXIMUM NUMBER OF PARAMETERS FOR OPTIMISATION. */
/* N2ELEC MAXIMUM NUMBER OF TWO ELECTRON INTEGRALS ALLOWED. */
/* MPACK AREA OF LOWER HALF TRIANGLE OF DENSITY MATRIX. */
/* MORB2 SQUARE OF THE MAXIMUM NUMBER OF ORBITALS ALLOWED. */
/* MAXHES AREA OF HESSIAN MATRIX */
/* MAXALL LARGER THAN MAXORB OR MAXPAR. */
/* *********************************************************************** */
/* *********************************************************************** */
/* DECK MOPAC */
/* COSMO change */
/* end of COSMO change */
/* *********************************************************************** */
/* HCORE GENERATES THE ONE-ELECTRON MATRIX AND TWO ELECTRON INTEGRALS */
/* FOR A GIVEN MOLECULE WHOSE GEOMETRY IS DEFINED IN CARTESIAN */
/* COORDINATES. */
/* ON INPUT COORD = COORDINATES OF THE MOLECULE. */
/* ON OUTPUT H = ONE-ELECTRON MATRIX. */
/* W = TWO-ELECTRON INTEGRALS. */
/* ENUCLR = NUCLEAR ENERGY */
/* *********************************************************************** */
/* Parameter adjustments */
--wk;
--wj;
--w;
--h__;
coord -= 4;
/* Function Body */
first = icalcn != numcal_1.numcal;
icalcn = numcal_1.numcal;
if (first) {
ione = 1;
cutoff = 1e10;
if (euler_1.id != 0) {
cutoff = 60.;
}
if (euler_1.id != 0) {
ione = 0;
}
debug = i_indx(keywrd_1.keywrd, "HCORE", (ftnlen)241, (ftnlen)5) != 0;
/* ****************************************************************** */
xf = 0.;
yf = 0.;
zf = 0.;
s_copy(tmpkey, keywrd_1.keywrd, (ftnlen)241, (ftnlen)241);
i__ = i_indx(tmpkey, " FIELD(", (ftnlen)241, (ftnlen)7);
if (i__ == 0) {
goto L6;
}
/* ERASE ALL TEXT FROM TMPKEY EXCEPT FIELD DATA */
s_copy(tmpkey, " ", i__, (ftnlen)1);
i__1 = i_indx(tmpkey, ")", (ftnlen)241, (ftnlen)1) - 1;
s_copy(tmpkey + i__1, " ", 241 - i__1, (ftnlen)1);
/* READ IN THE EFFECTIVE FIELD IN X,Y,Z COORDINATES */
xf = reada_(tmpkey, &i__, (ftnlen)241);
i__ = i_indx(tmpkey, ",", (ftnlen)241, (ftnlen)1);
if (i__ == 0) {
goto L5;
}
*(unsigned char *)&tmpkey[i__ - 1] = ' ';
yf = reada_(tmpkey, &i__, (ftnlen)241);
i__ = i_indx(tmpkey, ",", (ftnlen)241, (ftnlen)1);
if (i__ == 0) {
goto L5;
}
*(unsigned char *)&tmpkey[i__ - 1] = ' ';
zf = reada_(tmpkey, &i__, (ftnlen)241);
L5:
s_wsfe(&io___11);
do_fio(&c__1, (char *)&xf, (ftnlen)sizeof(doublereal));
do_fio(&c__1, (char *)&yf, (ftnlen)sizeof(doublereal));
do_fio(&c__1, (char *)&zf, (ftnlen)sizeof(doublereal));
e_wsfe();
s_wsfe(&io___12);
e_wsfe();
L6:
field_1.efield[0] = xf;
field_1.efield[1] = yf;
field_1.efield[2] = zf;
/* ********************************************************************** */
}
fldon = FALSE_;
if (field_1.efield[0] != 0. || field_1.efield[1] != 0. || field_1.efield[
2] != 0.) {
fldcon = 51.4257;
fldon = TRUE_;
}
i__1 = molkst_1.norbs * (molkst_1.norbs + 1) / 2;
for (i__ = 1; i__ <= i__1; ++i__) {
/* L10: */
h__[i__] = 0.;
}
*enuclr = 0.;
kr = 1;
i__1 = molkst_1.numat;
for (i__ = 1; i__ <= i__1; ++i__) {
ia = molkst_1.nfirst[i__ - 1];
ib = molkst_1.nlast[i__ - 1];
ic = molkst_1.nmidle[i__ - 1];
ni = molkst_1.nat[i__ - 1];
/* FIRST WE FILL THE DIAGONALS, AND OFF-DIAGONALS ON THE SAME ATOM */
i__2 = ib;
for (i1 = ia; i1 <= i__2; ++i1) {
i2 = i1 * (i1 - 1) / 2 + ia - 1;
i__3 = i1;
for (j1 = ia; j1 <= i__3; ++j1) {
++i2;
h__[i2] = 0.;
if (fldon) {
io1 = i1 - ia;
jo1 = j1 - ia;
if (jo1 == 0 && io1 == 1) {
hterme = multip_1.dd[ni - 1] * -.529177 *
field_1.efield[0] * fldcon;
h__[i2] = hterme;
}
if (jo1 == 0 && io1 == 2) {
hterme = multip_1.dd[ni - 1] * -.529177 *
field_1.efield[1] * fldcon;
h__[i2] = hterme;
}
if (jo1 == 0 && io1 == 3) {
hterme = multip_1.dd[ni - 1] * -.529177 *
field_1.efield[2] * fldcon;
h__[i2] = hterme;
}
}
/* L20: */
}
h__[i2] = molorb_1.uspd[i1 - 1];
if (fldon) {
fnuc = -(field_1.efield[0] * coord[i__ * 3 + 1] +
field_1.efield[1] * coord[i__ * 3 + 2] +
field_1.efield[2] * coord[i__ * 3 + 3]) * fldcon;
h__[i2] += fnuc;
}
/* L30: */
}
/* FILL THE ATOM-OTHER ATOM ONE-ELECTRON MATRIX<PSI(LAMBDA)|PSI(SIGMA)> */
im1 = i__ - ione;
i__2 = im1;
for (j = 1; j <= i__2; ++j) {
half = 1.;
if (i__ == j) {
half = .5;
}
ja = molkst_1.nfirst[j - 1];
jb = molkst_1.nlast[j - 1];
jc = molkst_1.nmidle[j - 1];
nj = molkst_1.nat[j - 1];
h1elec_(&ni, &nj, &coord[i__ * 3 + 1], &coord[j * 3 + 1], di);
i2 = 0;
i__3 = ib;
for (i1 = ia; i1 <= i__3; ++i1) {
ii = i1 * (i1 - 1) / 2 + ja - 1;
++i2;
j2 = 0;
jj = min(i1,jb);
i__4 = jj;
for (j1 = ja; j1 <= i__4; ++j1) {
++ii;
++j2;
/* L40: */
h__[ii] += di[i2 + j2 * 9 - 10];
}
}
/* CALCULATE THE TWO-ELECTRON INTEGRALS, W; THE ELECTRON NUCLEAR TERMS */
/* E1B AND E2A; AND THE NUCLEAR-NUCLEAR TERM ENUC. */
if (euler_1.id == 0) {
rotate_(&ni, &nj, &coord[i__ * 3 + 1], &coord[j * 3 + 1], &w[
kr], &kr, e1b, e2a, &enuc, &cutoff);
} else {
kro = kr;
solrot_(&ni, &nj, &coord[i__ * 3 + 1], &coord[j * 3 + 1], wjd,
wkd, &kr, e1b, e2a, &enuc, &cutoff);
jj = 0;
i__4 = kr - 1;
for (ii = kro; ii <= i__4; ++ii) {
++jj;
wj[ii] = wjd[jj - 1];
/* L50: */
wk[ii] = wkd[jj - 1];
}
}
*enuclr += enuc;
/* ADD ON THE ELECTRON-NUCLEAR ATTRACTION TERM FOR ATOM I. */
i2 = 0;
i__4 = ic;
for (i1 = ia; i1 <= i__4; ++i1) {
ii = i1 * (i1 - 1) / 2 + ia - 1;
i__3 = i1;
for (j1 = ia; j1 <= i__3; ++j1) {
++ii;
++i2;
/* L60: */
h__[ii] += e1b[i2 - 1] * half;
}
}
i__3 = ib;
for (i1 = ic + 1; i1 <= i__3; ++i1) {
ii = i1 * (i1 + 1) / 2;
/* L70: */
h__[ii] += e1b[0] * half;
}
/* ADD ON THE ELECTRON-NUCLEAR ATTRACTION TERM FOR ATOM J. */
i2 = 0;
i__3 = jc;
for (i1 = ja; i1 <= i__3; ++i1) {
ii = i1 * (i1 - 1) / 2 + ja - 1;
i__4 = i1;
for (j1 = ja; j1 <= i__4; ++j1) {
++ii;
++i2;
/* L80: */
h__[ii] += e2a[i2 - 1] * half;
}
}
i__4 = jb;
for (i1 = jc + 1; i1 <= i__4; ++i1) {
ii = i1 * (i1 + 1) / 2;
/* L90: */
h__[ii] += e2a[0] * half;
}
/* L100: */
}
/* L110: */
}
/* COSMO change */
/* A. KLAMT 16.7.91 */
if (iseps_1.useps) {
/* The following routine adds the dielectric correction for the electron-core */
/* interaction to the diagonal elements of H */
addhcr_(&h__[1]);
/* In the following routine the dielectric correction to the core-core- */
/* interaction is added to ENUCLR */
addnuc_(enuclr);
}
/* end of COSMO change */
if (! debug) {
return 0;
}
s_wsfe(&io___44);
e_wsfe();
vecprt_(&h__[1], &molkst_1.norbs);
j = min(400,kr);
if (euler_1.id == 0) {
s_wsfe(&io___45);
e_wsfe();
s_wsfe(&io___46);
i__1 = j;
for (i__ = 1; i__ <= i__1; ++i__) {
do_fio(&c__1, (char *)&w[i__], (ftnlen)sizeof(doublereal));
}
e_wsfe();
} else {
s_wsfe(&io___47);
e_wsfe();
s_wsfe(&io___48);
i__1 = j;
for (i__ = 1; i__ <= i__1; ++i__) {
do_fio(&c__1, (char *)&wj[i__], (ftnlen)sizeof(doublereal));
}
e_wsfe();
s_wsfe(&io___49);
e_wsfe();
s_wsfe(&io___50);
i__1 = j;
for (i__ = 1; i__ <= i__1; ++i__) {
do_fio(&c__1, (char *)&wk[i__], (ftnlen)sizeof(doublereal));
}
e_wsfe();
}
return 0;
} /* hcore_ */
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
/* Subroutine */ int solrot_(integer *ni, integer *nj, doublereal *xi,
doublereal *xj, doublereal *wj, doublereal *wk, integer *kr,
doublereal *e1b, doublereal *e2a, doublereal *enuc, doublereal *
cutoff)
{
/* Initialized data */
static integer icalcn = 0;
/* System generated locals */
integer i__1, i__2, i__3, i__4;
/* Local variables */
static integer i__, j, k, l, kb, ii;
static doublereal one;
#define lims ((integer *)&ucell_1)
static doublereal xjuc[3], wmax[100], wsum[100], wbits[100], e1bits[10],
e2bits[10], enubit;
extern /* Subroutine */ int rotate_(integer *, integer *, doublereal *,
doublereal *, doublereal *, integer *, doublereal *, doublereal *,
doublereal *, doublereal *);
/* *********************************************************************** */
/* SOLROT FORMS THE TWO-ELECTRON TWO-ATOM J AND K INTEGRAL STRINGS. */
/* ON EXIT WJ = "J"-TYPE INTEGRALS */
/* WK = "K"-TYPE INTEGRALS */
/* FOR MOLECULES, WJ = WK. */
/* *********************************************************************** */
/* Parameter adjustments */
--e2a;
--e1b;
--wk;
--wj;
--xj;
--xi;
/* Function Body */
if (icalcn != numcal_1.numcal) {
icalcn = numcal_1.numcal;
/* $DOIT ASIS */
i__1 = euler_1.id;
for (i__ = 1; i__ <= i__1; ++i__) {
lims[i__ - 1] = -1;
/* L10: */
lims[i__ + 2] = 1;
}
/* $DOIT ASIS */
for (i__ = euler_1.id + 1; i__ <= 3; ++i__) {
lims[i__ - 1] = 0;
/* L20: */
lims[i__ + 2] = 0;
}
}
one = 1.;
if (xi[1] == xj[1] && xi[2] == xj[2] && xi[3] == xj[3]) {
one = .5;
}
for (i__ = 1; i__ <= 100; ++i__) {
wmax[i__ - 1] = 0.;
wsum[i__ - 1] = 0.;
/* L30: */
wbits[i__ - 1] = 0.;
}
for (i__ = 1; i__ <= 10; ++i__) {
e1b[i__] = 0.;
/* L40: */
e2a[i__] = 0.;
}
*enuc = 0.;
i__1 = ucell_1.l1u;
for (i__ = ucell_1.l1l; i__ <= i__1; ++i__) {
i__2 = ucell_1.l2u;
for (j = ucell_1.l2l; j <= i__2; ++j) {
i__3 = ucell_1.l3u;
for (k = ucell_1.l3l; k <= i__3; ++k) {
/* $DOIT ASIS */
for (l = 1; l <= 3; ++l) {
/* L50: */
xjuc[l - 1] = xj[l] + euler_1.tvec[l - 1] * i__ +
euler_1.tvec[l + 2] * j + euler_1.tvec[l + 5] * k;
}
kb = 1;
rotate_(ni, nj, &xi[1], xjuc, wbits, &kb, e1bits, e2bits, &
enubit, cutoff);
--kb;
i__4 = kb;
for (ii = 1; ii <= i__4; ++ii) {
/* L60: */
wsum[ii - 1] += wbits[ii - 1];
}
if (wmax[0] < wbits[0]) {
i__4 = kb;
for (ii = 1; ii <= i__4; ++ii) {
/* L70: */
wmax[ii - 1] = wbits[ii - 1];
}
}
for (ii = 1; ii <= 10; ++ii) {
e1b[ii] += e1bits[ii - 1];
/* L80: */
e2a[ii] += e2bits[ii - 1];
}
*enuc += enubit * one;
/* L90: */
}
}
}
if (one < .9) {
i__3 = kb;
for (i__ = 1; i__ <= i__3; ++i__) {
/* L100: */
wmax[i__ - 1] = 0.;
}
}
i__3 = kb;
for (i__ = 1; i__ <= i__3; ++i__) {
wk[i__] = wmax[i__ - 1];
/* L110: */
wj[i__] = wsum[i__ - 1];
}
*kr = kb + *kr;
return 0;
} /* solrot_ */
/* Subroutine */ int dhcore_(doublereal *coord, doublereal *h__, doublereal *
w, doublereal *enuclr, integer *nati, integer *natx, doublereal *step)
{
/* Initialized data */
static integer nb[9] = { 1,0,0,10,0,0,0,0,45 };
static logical first = TRUE_;
/* System generated locals */
integer i__1, i__2, i__3;
/* Builtin functions */
integer i_indx(char *, char *, ftnlen, ftnlen);
/* Local variables */
static integer i__, j, k, i1, i2, j2, j1, j7, ia, ib, ic;
static doublereal di[81] /* was [9][9] */;
static integer ja, jb, jc, ii, ij, ni, nj, kr;
static doublereal e1b[10], e2a[10], ddi[81] /* was [9][9] */, wjd[101];
static integer kro;
static doublereal de1b[10], de2a[10], dwjd[101], enuc;
static integer nrow;
static doublereal denuc, csave;
static logical mindo;
extern /* Subroutine */ int h1elec_(integer *, integer *, doublereal *,
doublereal *, doublereal *);
static integer nband2;
static doublereal cutoff;
extern /* Subroutine */ int rotate_(integer *, integer *, doublereal *,
doublereal *, doublereal *, integer *, doublereal *, doublereal *,
doublereal *, doublereal *);
/* COMDECK SIZES */
/* *********************************************************************** */
/* THIS FILE CONTAINS ALL THE ARRAY SIZES FOR USE IN MOPAC. */
/* THERE ARE ONLY 5 PARAMETERS THAT THE PROGRAMMER NEED SET: */
/* MAXHEV = MAXIMUM NUMBER OF HEAVY ATOMS (HEAVY: NON-HYDROGEN ATOMS) */
/* MAXLIT = MAXIMUM NUMBER OF HYDROGEN ATOMS. */
/* MAXTIM = DEFAULT TIME FOR A JOB. (SECONDS) */
/* MAXDMP = DEFAULT TIME FOR AUTOMATIC RESTART FILE GENERATION (SECS) */
/* ISYBYL = 1 IF MOPAC IS TO BE USED IN THE SYBYL PACKAGE, =0 OTHERWISE */
/* SEE ALSO NMECI, NPULAY AND MESP AT THE END OF THIS FILE */
/* *********************************************************************** */
/* THE FOLLOWING CODE DOES NOT NEED TO BE ALTERED BY THE PROGRAMMER */
/* *********************************************************************** */
/* ALL OTHER PARAMETERS ARE DERIVED FUNCTIONS OF THESE TWO PARAMETERS */
/* NAME DEFINITION */
/* NUMATM MAXIMUM NUMBER OF ATOMS ALLOWED. */
/* MAXORB MAXIMUM NUMBER OF ORBITALS ALLOWED. */
/* MAXPAR MAXIMUM NUMBER OF PARAMETERS FOR OPTIMISATION. */
/* N2ELEC MAXIMUM NUMBER OF TWO ELECTRON INTEGRALS ALLOWED. */
/* MPACK AREA OF LOWER HALF TRIANGLE OF DENSITY MATRIX. */
/* MORB2 SQUARE OF THE MAXIMUM NUMBER OF ORBITALS ALLOWED. */
/* MAXHES AREA OF HESSIAN MATRIX */
/* MAXALL LARGER THAN MAXORB OR MAXPAR. */
/* *********************************************************************** */
/* *********************************************************************** */
/* DECK MOPAC */
/* DHCORE GENERATES THE 1-ELECTRON AND 2-ELECTRON INTEGRALS DERIVATIVES */
/* WITH RESPECT TO THE CARTESIAN COORDINATE COORD (NATX,NATI). */
/* INPUT */
/* COORD : CARTESIAN COORDINATES OF THE MOLECULE. */
/* NATI,NATX : INDICES OF THE MOVING COORDINATE. */
/* STEP : STEP SIZE OF THE 2-POINTS FINITE DIFFERENCE. */
/* OUTPUT */
/* H : 1-ELECTRON INTEGRALS DERIVATIVES (PACKED CANONICAL). */
/* W : 2-ELECTRON INTEGRALS DERIVATIVES (ORDERED AS REQUIRED */
/* IN DFOCK2 AND DIJKL1). */
/* ENUCLR : NUCLEAR ENERGY DERIVATIVE. */
/* Parameter adjustments */
--w;
--h__;
coord -= 4;
/* Function Body */
if (first) {
cutoff = 1e10;
first = FALSE_;
mindo = i_indx(keywrd_1.keywrd, "MINDO", (ftnlen)241, (ftnlen)5) != 0;
}
i__1 = molkst_2.norbs * (molkst_2.norbs + 1) / 2;
for (i__ = 1; i__ <= i__1; ++i__) {
/* L10: */
h__[i__] = 0.;
}
*enuclr = 0.;
kr = 1;
nrow = 0;
i__ = *nati;
csave = coord[*natx + *nati * 3];
ia = molkst_2.nfirst[*nati - 1];
ib = molkst_2.nlast[*nati - 1];
ic = molkst_2.nmidle[*nati - 1];
ni = molkst_2.nat[*nati - 1];
nrow = -nb[ib - ia];
i__1 = molkst_2.numat;
for (j = 1; j <= i__1; ++j) {
/* L20: */
nrow += nb[molkst_2.nlast[j - 1] - molkst_2.nfirst[j - 1]];
}
/* # NCOL=NB(NLAST(NATI)-NFIRST(NATI)) */
nband2 = 0;
i__1 = molkst_2.numat;
for (j = 1; j <= i__1; ++j) {
if (j == *nati) {
goto L120;
}
ja = molkst_2.nfirst[j - 1];
jb = molkst_2.nlast[j - 1];
jc = molkst_2.nmidle[j - 1];
nj = molkst_2.nat[j - 1];
coord[*natx + *nati * 3] = csave + *step;
h1elec_(&ni, &nj, &coord[*nati * 3 + 1], &coord[j * 3 + 1], di);
/* THE FOLLOWING STYLE WAS NECESSARY TO GET ROUND A BUG IN THE */
/* GOULD COMPILER */
coord[*natx + *nati * 3] = csave + *step * -1.;
h1elec_(&ni, &nj, &coord[*nati * 3 + 1], &coord[j * 3 + 1], ddi);
/* FILL THE ATOM-OTHER ATOM ONE-ELECTRON MATRIX. */
i2 = 0;
if (ia > ja) {
i__2 = ib;
for (i1 = ia; i1 <= i__2; ++i1) {
ij = i1 * (i1 - 1) / 2 + ja - 1;
++i2;
j2 = 0;
i__3 = jb;
for (j1 = ja; j1 <= i__3; ++j1) {
++ij;
++j2;
/* L30: */
h__[ij] += di[i2 + j2 * 9 - 10] - ddi[i2 + j2 * 9 - 10];
}
}
} else {
i__3 = jb;
for (i1 = ja; i1 <= i__3; ++i1) {
ij = i1 * (i1 - 1) / 2 + ia - 1;
++i2;
j2 = 0;
i__2 = ib;
for (j1 = ia; j1 <= i__2; ++j1) {
++ij;
++j2;
/* L40: */
h__[ij] += di[j2 + i2 * 9 - 10] - ddi[j2 + i2 * 9 - 10];
}
}
}
/* CALCULATE THE TWO-ELECTRON INTEGRALS, W; THE ELECTRON NUCLEAR TERM */
/* E1B AND E2A; AND THE NUCLEAR-NUCLEAR TERM ENUC. */
kro = kr;
nband2 += nb[molkst_2.nlast[j - 1] - molkst_2.nfirst[j - 1]];
if (mindo) {
coord[*natx + *nati * 3] = csave + *step;
rotate_(&ni, &nj, &coord[*nati * 3 + 1], &coord[j * 3 + 1], wjd, &
kr, e1b, e2a, &enuc, &cutoff);
kr = kro;
coord[*natx + *nati * 3] = csave + *step * -1.;
rotate_(&ni, &nj, &coord[*nati * 3 + 1], &coord[j * 3 + 1], dwjd,
&kr, de1b, de2a, &denuc, &cutoff);
if (kr > kro) {
i__2 = kr - kro + 1;
for (k = 1; k <= i__2; ++k) {
/* L50: */
w[kro + k - 1] = wjd[k - 1] - dwjd[k - 1];
}
}
} else {
coord[*natx + *nati * 3] = csave + *step;
rotate_(&ni, &nj, &coord[*nati * 3 + 1], &coord[j * 3 + 1], wjd, &
kr, e1b, e2a, &enuc, &cutoff);
kr = kro;
coord[*natx + *nati * 3] = csave + *step * -1.;
rotate_(&ni, &nj, &coord[*nati * 3 + 1], &coord[j * 3 + 1], dwjd,
&kr, de1b, de2a, &denuc, &cutoff);
if (kr > kro) {
i__2 = kr - kro + 1;
for (k = 1; k <= i__2; ++k) {
/* L60: */
wjd[k - 1] -= dwjd[k - 1];
}
j7 = 0;
i__2 = kr;
for (i1 = kro; i1 <= i__2; ++i1) {
++j7;
/* L70: */
w[i1] = wjd[j7 - 1];
}
}
}
coord[*natx + *nati * 3] = csave;
*enuclr = *enuclr + enuc - denuc;
/* ADD ON THE ELECTRON-NUCLEAR ATTRACTION TERM FOR ATOM I. */
i2 = 0;
i__2 = ic;
for (i1 = ia; i1 <= i__2; ++i1) {
ii = i1 * (i1 - 1) / 2 + ia - 1;
i__3 = i1;
for (j1 = ia; j1 <= i__3; ++j1) {
++ii;
++i2;
/* L80: */
h__[ii] = h__[ii] + e1b[i2 - 1] - de1b[i2 - 1];
}
}
/* CONTRIB D, CNDO. */
i__3 = ib;
for (i1 = ic + 1; i1 <= i__3; ++i1) {
ii = i1 * (i1 + 1) / 2;
/* L90: */
h__[ii] = h__[ii] + e1b[0] - de1b[0];
}
/* ADD ON THE ELECTRON-NUCLEAR ATTRACTION TERM FOR ATOM J. */
i2 = 0;
i__3 = jc;
for (i1 = ja; i1 <= i__3; ++i1) {
ii = i1 * (i1 - 1) / 2 + ja - 1;
i__2 = i1;
for (j1 = ja; j1 <= i__2; ++j1) {
++ii;
++i2;
/* L100: */
h__[ii] = h__[ii] + e2a[i2 - 1] - de2a[i2 - 1];
}
}
/* CONTRIB D, CNDO. */
i__2 = jb;
for (i1 = jc + 1; i1 <= i__2; ++i1) {
ii = i1 * (i1 + 1) / 2;
/* L110: */
h__[ii] = h__[ii] + e2a[0] - de2a[0];
}
L120:
;
}
/* 'SIZE' OF H IS NROW * NCOL */
return 0;
} /* dhcore_ */
