//find u such that truncation(u) < accx and truncation(u / 1.2) > accx
void findu(qfvars *vars, REAL8* utx, REAL8 accx)
{
REAL8 u, ut;
REAL8 divis[] = {2.0, 1.4, 1.2, 1.1};
ut = *utx;
u = 0.25*ut;
if ( truncation(vars, u, 0.0)>accx ) {
//for ( u=ut; truncation(vars, u, 0.0)>accx; u=ut) ut *= 4.0;
u = ut;
while (truncation(vars, u, 0.0)>accx) {
ut *= 4.0;
u = ut;
}
} else {
ut = u;
//for ( u=0.25*u; truncation(vars, u, 0.0) <= accx; u=0.25*u ) ut = u;
u *= 0.25;
while (truncation(vars, u, 0.0) <= accx) {
ut = u;
u *= 0.25;
}
}
for (UINT4 ii=0; ii<4; ii++) {
u = ut/divis[ii];
if ( truncation(vars, u, 0.0)<=accx ) ut = u;
}
*utx = ut;
} /* findu() */
/** first
0 - FALSE - call to handel nodes with next priority / order
1 - TRUE (default) - first call for this object
*/
bool INode::childTopDown(bool first)
{
#ifdef DEBUGMSG
qDebug("# INode::childTopDown (%s) cont_td (this %p) first: %d", (const char *) name(), this, first);
qDebug("##(xxx) INode::childTopDown (%s) (this %p) #childcount %d, ordercount %d, aktivorder %d, aktivcount %d, truncation %d",
(const char *) name(), this, childcount_,ordercount_,aktivorder_,aktivcount_,truncation());
#endif
SNode *el; // hilfspointer
if (first) {// first call of 'childTopDown' for this object
childList_ = &(sNode_->children()); // liste der subknoten erzeugen
childcount_ = childList_->count(); // anzahl aller (noch) zu bearbeitenden knoten
aktivorder_=-1; // prioritaet der zu bearbeitenden nodes
if (childcount_ == 0) { //no sub nodes
aktivcount_ = 0;
execBottomUp(); //execState(BU);// BottomUp
return 0;
}
} else { // recursiv call of 'childTopDown' for this object
if (aktivcount_ > 0 || ordercount_ > 0 ) return 1;
if (childcount_ <= 0) return 0;//all sub nodes are handled
if (truncation()) return 0; //all sub nodes are handelt or node is trash
}
//Praemisse (ordercount_ == 0) && (childcount_ > 0)
while (ordercount_ == 0) {
aktivorder_ ++;
for (el = childList_->first(); el != 0; el = childList_->next())
if(el->order() == aktivorder_) ordercount_++; // anzahl der nodes der aktuellen prioritaet
}
#ifdef DEBUGMSG
qDebug("## INode::childTopDown (%s) (this %p) #childs %d, ordercount %d, aktivorder %d, aktivcount_ %d",
(const char *) name(), this, childcount_, ordercount_, aktivorder_,aktivcount_);
#endif
INode *inode;
for (el = childList_->first(); el != 0; el = childList_->next()) {
if (truncation()) { //bedingungen nicht mehr erfüllt
aktivcount_ = 0;
execBottomUp(); //execState(BU);// BottomUp
return 0;
}
if (el->order() == aktivorder_) {
incrementCount(); //einer mehr in der queue -> siehe decrementCount()
childcount_--; ordercount_--; //aktuelle wird gleich bearbeitet
inode = new INode(el); //new INode
CHECK_PTR(inode);
inode->status(HI);
inode->execState(TD);
childLink(inode); // remount node in the tree
inode->execTopDown(); //start topdown operator
//childcount_--; ordercount_--; //aktuelle wird gleich bearbeitet
//!MP25.07.2001 inode might be deleted
//! analysis_->nodeChange(inode); //info to the rest of the world
analysis_->nodeChange(0);
}
}
return 1;
}
void SVGInlineTextBox::paintSelectionBackground(PaintInfo& paintInfo)
{
ASSERT(paintInfo.shouldPaintWithinRoot(renderer()));
ASSERT(paintInfo.phase == PaintPhaseForeground || paintInfo.phase == PaintPhaseSelection);
ASSERT(truncation() == cNoTruncation);
if (renderer()->style()->visibility() != VISIBLE)
return;
RenderObject* parentRenderer = parent()->renderer();
ASSERT(parentRenderer);
ASSERT(!parentRenderer->document().printing());
// Determine whether or not we're selected.
bool paintSelectedTextOnly = paintInfo.phase == PaintPhaseSelection;
bool hasSelection = selectionState() != RenderObject::SelectionNone;
if (!hasSelection || paintSelectedTextOnly)
return;
Color backgroundColor = renderer()->selectionBackgroundColor();
if (!backgroundColor.alpha())
return;
RenderSVGInlineText* textRenderer = toRenderSVGInlineText(this->textRenderer());
ASSERT(textRenderer);
if (!textShouldBePainted(textRenderer))
return;
RenderStyle* style = parentRenderer->style();
ASSERT(style);
int startPosition, endPosition;
selectionStartEnd(startPosition, endPosition);
int fragmentStartPosition = 0;
int fragmentEndPosition = 0;
AffineTransform fragmentTransform;
unsigned textFragmentsSize = m_textFragments.size();
for (unsigned i = 0; i < textFragmentsSize; ++i) {
SVGTextFragment& fragment = m_textFragments.at(i);
ASSERT(!m_paintingResource);
fragmentStartPosition = startPosition;
fragmentEndPosition = endPosition;
if (!mapStartEndPositionsIntoFragmentCoordinates(fragment, fragmentStartPosition, fragmentEndPosition))
continue;
GraphicsContextStateSaver stateSaver(*paintInfo.context);
fragment.buildFragmentTransform(fragmentTransform);
if (!fragmentTransform.isIdentity())
paintInfo.context->concatCTM(fragmentTransform);
paintInfo.context->setFillColor(backgroundColor);
paintInfo.context->fillRect(selectionRectForTextFragment(fragment, fragmentStartPosition, fragmentEndPosition, style), backgroundColor);
m_paintingResourceMode = ApplyToDefaultMode;
}
ASSERT(!m_paintingResource);
}
/** decrement the aktiv counter * aktiv TopDown child operators
++ RETURN:
* -1 -> ERROR (aktivcount < 0)
* 0 -> aktivcount == 0
* 1 -> aktivcount > 0
* "this" in not a last node*/
int INode::decrementCount()
{
#ifdef DEBUGMSG
qDebug("# decrementCount(%s)(%p): Start, aktivcount = %d\n",
(const char *) name(), this, aktivcount_);
#endif
analysis_->nodeChange(this);
if (aktivcount_ == 0) {
#ifdef DEBUGMSG
qDebug("# (ERROR) INode::decrementCount < 0!!\n");
#endif
return -1;
}
aktivcount_--;
if (!childTopDown(FALSE)) {//return FALSE when all sub nodes are handeled or node is trash
if (truncation() && !isRoot()) {
parent()->decrementCount();
} else {
execBottomUp();// jetzt geht es wieder nach oben
}
return 0;
}
return 1;
}
static void findu(double* utx, double accx)
/* find u such that truncation(u) < accx and truncation(u / 1.2) > accx */
{
double u, ut; int i;
static double divis[]={2.0,1.4,1.2,1.1};
ut = *utx; u = ut / 4.0;
if ( truncation(u, 0.0) > accx )
{
for ( u = ut; truncation(u, 0.0) > accx; u = ut) ut = ut * 4.0;
}
else
{
ut = u;
for ( u = u / 4.0; truncation(u, 0.0) <= accx; u = u / 4.0 )
ut = u;
}
for ( i=0;i<4;i++)
{ u = ut/divis[i]; if ( truncation(u, 0.0) <= accx ) ut = u; }
*utx = ut;
}
void SVGInlineTextBox::paint(PaintInfo& paintInfo, const LayoutPoint& paintOffset, LayoutUnit, LayoutUnit)
{
ASSERT(paintInfo.shouldPaintWithinRoot(renderer()));
ASSERT(paintInfo.phase == PaintPhaseForeground || paintInfo.phase == PaintPhaseSelection);
ASSERT(truncation() == cNoTruncation);
if (renderer()->style()->visibility() != VISIBLE)
return;
// Note: We're explicitely not supporting composition & custom underlines and custom highlighters - unlike InlineTextBox.
// If we ever need that for SVG, it's very easy to refactor and reuse the code.
RenderObject* parentRenderer = parent()->renderer();
ASSERT(parentRenderer);
bool paintSelectedTextOnly = paintInfo.phase == PaintPhaseSelection;
bool hasSelection = !parentRenderer->document().printing() && selectionState() != RenderObject::SelectionNone;
if (!hasSelection && paintSelectedTextOnly)
return;
RenderSVGInlineText* textRenderer = toRenderSVGInlineText(this->textRenderer());
ASSERT(textRenderer);
if (!textShouldBePainted(textRenderer))
return;
RenderStyle* style = parentRenderer->style();
ASSERT(style);
paintDocumentMarkers(paintInfo.context, paintOffset, style, textRenderer->scaledFont(), true);
const SVGRenderStyle* svgStyle = style->svgStyle();
ASSERT(svgStyle);
bool hasFill = svgStyle->hasFill();
bool hasVisibleStroke = svgStyle->hasVisibleStroke();
RenderStyle* selectionStyle = style;
if (hasSelection) {
selectionStyle = parentRenderer->getCachedPseudoStyle(SELECTION);
if (selectionStyle) {
const SVGRenderStyle* svgSelectionStyle = selectionStyle->svgStyle();
ASSERT(svgSelectionStyle);
if (!hasFill)
hasFill = svgSelectionStyle->hasFill();
if (!hasVisibleStroke)
hasVisibleStroke = svgSelectionStyle->hasVisibleStroke();
} else
selectionStyle = style;
}
if (textRenderer->frame() && textRenderer->frame()->view() && textRenderer->frame()->view()->paintBehavior() & PaintBehaviorRenderingSVGMask) {
hasFill = true;
hasVisibleStroke = false;
}
AffineTransform fragmentTransform;
unsigned textFragmentsSize = m_textFragments.size();
for (unsigned i = 0; i < textFragmentsSize; ++i) {
SVGTextFragment& fragment = m_textFragments.at(i);
ASSERT(!m_paintingResource);
GraphicsContextStateSaver stateSaver(*paintInfo.context, false);
fragment.buildFragmentTransform(fragmentTransform);
if (!fragmentTransform.isIdentity()) {
stateSaver.save();
paintInfo.context->concatCTM(fragmentTransform);
}
// Spec: All text decorations except line-through should be drawn before the text is filled and stroked; thus, the text is rendered on top of these decorations.
unsigned decorations = style->textDecorationsInEffect();
if (decorations & TextDecorationUnderline)
paintDecoration(paintInfo.context, TextDecorationUnderline, fragment);
if (decorations & TextDecorationOverline)
paintDecoration(paintInfo.context, TextDecorationOverline, fragment);
for (int i = 0; i < 3; i++) {
switch (svgStyle->paintOrderType(i)) {
case PT_FILL:
// Fill text
if (hasFill) {
m_paintingResourceMode = ApplyToFillMode | ApplyToTextMode;
paintText(paintInfo.context, style, selectionStyle, fragment, hasSelection, paintSelectedTextOnly);
}
break;
case PT_STROKE:
// Stroke text
if (hasVisibleStroke) {
m_paintingResourceMode = ApplyToStrokeMode | ApplyToTextMode;
paintText(paintInfo.context, style, selectionStyle, fragment, hasSelection, paintSelectedTextOnly);
}
break;
case PT_MARKERS:
// Markers don't apply to text
break;
default:
ASSERT_NOT_REACHED();
break;
}
}
// Spec: Line-through should be drawn after the text is filled and stroked; thus, the line-through is rendered on top of the text.
if (decorations & TextDecorationLineThrough)
paintDecoration(paintInfo.context, TextDecorationLineThrough, fragment);
m_paintingResourceMode = ApplyToDefaultMode;
}
ASSERT(!m_paintingResource);
}
/** handle the results of the TopDown operator
* pid == 0 -> no operator or node have no holistic operator
* pid == -1 -> can't start process
* never call for "root" node (this is not root node)
* this can be "last" node
*/
int INode::evalTopDown(int pid)
{
#ifdef DEBUGMSG
qDebug("#* INode::evalTopDown(%s)(%p): pid=%d Start\n",
(const char *) name(), this, pid);
#endif
CHECK_PTR(sNode_);
//Hier Absturz weil parent = 0
CHECK_PTR(parent());
INode* parent=INode::parent();
if (pid <= 0) {
#ifdef DEBUGMSG
qDebug
("# (ERROR -1/0) evalTopDown: can't start process \n try it structural\n");
#endif
#warning Without Dummy TopDown nothing works
if (!isLast() && !truncation())
childTopDown(); //do it structural
else if (isLast()) { //is last node
status(MI);
execState(BU);
parent->childUnlink(this); //!remove this temporary INode
parent->decrementCount();
return 0;
} else { //is trash
status(TRASH);
execState(BU);
parent->childUnlink(this); //!remove this temporary INode
parent->decrementCount();
return 0;
}
}
if (sNode_->holistic()) { //pid > 0
QList < INode > iNodeList = sNode_->evalTopDown(this);
#ifdef DEBUGMSG
qDebug("##*** evalTopDown: %d(min) < %d(ist) < %d(max)",sNode()->minNumNode(),iNodeList.count(), sNode()->maxNumNode());
#endif
if (!(sNode()->minNumNode() == 0 && sNode()->maxNumNode() == 0 )) //beide eintraege null -> alles erlaubt
if (iNodeList.count() < sNode()->minNumNode() || iNodeList.count() > sNode()->maxNumNode() ) {
#ifdef DEBUGMSG
qDebug("***** iNodeList.count %d, sNode->minNumNode %d, sNode->maxNumNode %d \n",iNodeList.count(),sNode()->minNumNode(),sNode()->maxNumNode());
#endif
status(MI);
execState(BU);
truncation(TRUE);
//! unlink here
parent->status(TRASH);
parent->truncation(TRUE);
parent->childUnlink(this); //!remove this temporary INode
parent->decrementCount();
return 0;
}
#ifdef DEBUGMSG
qDebug("# (INFO) sNode is holistic; count new nodes: %d\n", iNodeList.count());
#endif
if (iNodeList.isEmpty()) {
#ifdef DEBUGMSG
qDebug("# (warning) operator return no result - missing Instance");
#endif
status(MI);
execState(BU);
//! unlink here
parent->childUnlink(this); //!remove this temporary INode
parent->decrementCount();
return 0;
}
parent->incrementCount(iNodeList.count() - 1);
INode *el;
parent->childUnlink(this); //!remove this temporary INode
for (el = iNodeList.first(); el != 0; el = iNodeList.next()) {
#ifdef DEBUGMSG
qDebug("# einhaengen: (%p) %s in %s", this,(const char *)el->name(), (const char *)parent->name());
#endif
if (el->isLast())
el->status(CI); //el->execState(BU); ???????????
else
el->status(PI);
el->execState(TD); //}?????????????
parent->childLink(el); //insert the new INodes
analysis_->nodeChange(el);
if (el->isLast())
parent->decrementCount();
else
el->childTopDown(); //last node have no children
}
//delete this; //XXXX
#ifdef DEBUGMSG
qDebug("# einhaengen: (%p) fertig in %s",this,(const char *)parent->name());
#endif
return 0;
}
else {
qDebug
("# (ERROR 1) taskFinished no operator found \n try it structural\n");
if (isLast())
qDebug("# (ERROR 1) last node must be holistic!\n");
parent->childUnlink(this); //!remove this temporary INode
return 0;
}
}
double qfc(double* lb1, double* nc1, int* n1, int *r1in, double *sigmain,
double *c1in, int *lim1in, double *accin, double* trace, int* ifault)
/* distribution function of a linear combination of non-central
chi-squared random variables :
input:
lb[j] coefficient of j-th chi-squared variable
nc[j] non-centrality parameter
n[j] degrees of freedom
j = 0, 2 ... r-1
sigma coefficient of standard normal variable
c point at which df is to be evaluated
lim maximum number of terms in integration
acc maximum error
output:
ifault = 1 required accuracy NOT achieved
2 round-off error possibly significant
3 invalid parameters
4 unable to locate integration parameters
5 out of memory
trace[0] absolute sum
trace[1] total number of integration terms
trace[2] number of integrations
trace[3] integration interval in final integration
trace[4] truncation point in initial integration
trace[5] s.d. of initial convergence factor
trace[6] cycles to locate integration parameters */
{
int r1 = r1in[0], lim1 = lim1in[0];
double sigma = sigmain[0], c1 = c1in[0], acc = accin[0];
int j, nj, nt, ntm; double acc1, almx, xlim, xnt, xntm;
double utx, tausq, sd, intv, intv1, x, up, un, d1, d2, lj, ncj;
double qfval = 0;
static int rats[]={1,2,4,8};
if (setjmp(env) != 0) { *ifault=4; goto endofproc; }
r=r1; lim=lim1; c=c1;
n=n1; lb=lb1; nc=nc1;
for ( j = 0; j<7; j++ ) trace[j] = 0.0;
*ifault = 0; count = 0;
intl = 0.0; ersm = 0.0;
qfval = -1.0; acc1 = acc; ndtsrt = TRUE; fail = FALSE;
xlim = (double)lim;
th=(int*)malloc(r*(sizeof(int)));
if (! th) { *ifault=5; goto endofproc; }
/* find mean, sd, max and min of lb,
check that parameter values are valid */
sigsq = square(sigma); sd = sigsq;
lmax = 0.0; lmin = 0.0; mean = 0.0;
for (j=0; j<r; j++ )
{
nj = n[j]; lj = lb[j]; ncj = nc[j];
if ( nj < 0 || ncj < 0.0 ) { *ifault = 3; goto endofproc; }
sd = sd + square(lj) * (2 * nj + 4.0 * ncj);
mean = mean + lj * (nj + ncj);
if (lmax < lj) lmax = lj ; else if (lmin > lj) lmin = lj;
}
if ( sd == 0.0 )
{ qfval = (c > 0.0) ? 1.0 : 0.0; goto endofproc; }
if ( lmin == 0.0 && lmax == 0.0 && sigma == 0.0 )
{ *ifault = 3; goto endofproc; }
sd = sqrt(sd);
almx = (lmax < - lmin) ? - lmin : lmax;
/* starting values for findu, ctff */
utx = 16.0 / sd; up = 4.5 / sd; un = - up;
/* truncation point with no convergence factor */
findu(&utx, .5 * acc1);
/* does convergence factor help */
if (c != 0.0 && (almx > 0.07 * sd))
{
tausq = .25 * acc1 / cfe(c);
if (fail) fail = FALSE ;
else if (truncation(utx, tausq) < .2 * acc1)
{
sigsq = sigsq + tausq;
findu(&utx, .25 * acc1);
trace[5] = sqrt(tausq);
}
}
trace[4] = utx; acc1 = 0.5 * acc1;
/* find RANGE of distribution, quit if outside this */
l1:
d1 = ctff(acc1, &up) - c;
if (d1 < 0.0) { qfval = 1.0; goto endofproc; }
d2 = c - ctff(acc1, &un);
if (d2 < 0.0) { qfval = 0.0; goto endofproc; }
/* find integration interval */
intv = 2.0 * pi / ((d1 > d2) ? d1 : d2);
/* calculate number of terms required for main and
auxillary integrations */
xnt = utx / intv; xntm = 3.0 / sqrt(acc1);
if (xnt > xntm * 1.5)
{
/* parameters for auxillary integration */
if (xntm > xlim) { *ifault = 1; goto endofproc; }
ntm = (int)floor(xntm+0.5);
intv1 = utx / ntm; x = 2.0 * pi / intv1;
if (x <= fabs(c)) goto l2;
/* calculate convergence factor */
tausq = .33 * acc1 / (1.1 * (cfe(c - x) + cfe(c + x)));
if (fail) goto l2;
acc1 = .67 * acc1;
/* auxillary integration */
integrate(ntm, intv1, tausq, FALSE );
xlim = xlim - xntm; sigsq = sigsq + tausq;
trace[2] = trace[2] + 1; trace[1] = trace[1] + ntm + 1;
/* find truncation point with new convergence factor */
findu(&utx, .25 * acc1); acc1 = 0.75 * acc1;
goto l1;
}
/* main integration */
l2:
trace[3] = intv;
if (xnt > xlim) { *ifault = 1; goto endofproc; }
nt = (int)floor(xnt+0.5);
integrate(nt, intv, 0.0, TRUE );
trace[2] = trace[2] + 1; trace[1] = trace[1] + nt + 1;
qfval = 0.5 - intl;
trace[0] = ersm;
/* test whether round-off error could be significant
allow for radix 8 or 16 machines */
up=ersm; x = up + acc / 10.0;
for (j=0;j<4;j++) { if (rats[j] * x == rats[j] * up) *ifault = 2; }
endofproc :
free((char*)th);
trace[6] = (double)count;
return qfval;
}
int main(int argc, char** argv)
{
ROS_INFO("Starting up chisel node.");
ros::init(argc, argv, "Chisel");
ros::NodeHandle nh("~");
int chunkSizeX, chunkSizeY, chunkSizeZ;
double voxelResolution;
double truncationDistQuad;
double truncationDistLinear;
double truncationDistConst;
double truncationDistScale;
int weight;
bool useCarving;
bool useColor;
double carvingDist;
std::string depthImageTopic;
std::string depthImageInfoTopic;
std::string depthImageTransform;
std::string colorImageTopic;
std::string colorImageInfoTopic;
std::string colorImageTransform;
std::string baseTransform;
std::string meshTopic;
std::string chunkBoxTopic;
double nearPlaneDist;
double farPlaneDist;
chisel_ros::ChiselServer::FusionMode mode;
std::string modeString;
std::string pointCloudTopic;
nh.param("chunk_size_x", chunkSizeX, 32);
nh.param("chunk_size_y", chunkSizeY, 32);
nh.param("chunk_size_z", chunkSizeZ, 32);
nh.param("truncation_constant", truncationDistConst, 0.001504);
nh.param("truncation_linear", truncationDistLinear, 0.00152);
nh.param("truncation_quadratic", truncationDistQuad, 0.0019);
nh.param("truncation_scale", truncationDistScale, 8.0);
nh.param("integration_weight", weight, 1);
nh.param("use_voxel_carving", useCarving, true);
nh.param("use_color", useColor, true);
nh.param("carving_dist_m", carvingDist, 0.05);
nh.param("voxel_resolution_m", voxelResolution, 0.03);
nh.param("near_plane_dist", nearPlaneDist, 0.05);
nh.param("far_plane_dist", farPlaneDist, 5.0);
nh.param("depth_image_topic", depthImageTopic, std::string("/depth_image"));
nh.param("point_cloud_topic", pointCloudTopic, std::string("/camera/depth_registered/points"));
nh.param("depth_image_info_topic", depthImageInfoTopic, std::string("/depth_camera_info"));
nh.param("depth_image_transform", depthImageTransform, std::string("/camera_depth_optical_frame"));
nh.param("color_image_topic", colorImageTopic, std::string("/color_image"));
nh.param("color_image_info_topic", colorImageInfoTopic, std::string("/color_camera_info"));
nh.param("color_image_transform", colorImageTransform, std::string("/camera_rgb_optical_frame"));
nh.param("base_transform", baseTransform, std::string("/camera_link"));
nh.param("mesh_topic", meshTopic, std::string("full_mesh"));
nh.param("chunk_box_topic", chunkBoxTopic, std::string("chunk_boxes"));
nh.param("fusion_mode", modeString, std::string("DepthImage"));
if(modeString == "DepthImage")
{
ROS_INFO("Mode depth image");
mode = chisel_ros::ChiselServer::FusionMode::DepthImage;
}
else if(modeString == "PointCloud")
{
ROS_INFO("Mode point cloud");
mode = chisel_ros::ChiselServer::FusionMode::PointCloud;
}
else
{
ROS_ERROR("Unrecognized fusion mode %s. Recognized modes: \"DepthImage\", \"PointCloud\"\n", modeString.c_str());
return -1;
}
ROS_INFO("Subscribing.");
chisel::Vec4 truncation(truncationDistQuad, truncationDistLinear, truncationDistConst, truncationDistScale);
chisel_ros::ChiselServerPtr server(new chisel_ros::ChiselServer(nh, chunkSizeX, chunkSizeY, chunkSizeZ, voxelResolution, useColor, mode));
server->SetupProjectionIntegrator(truncation, static_cast<uint16_t>(weight), useCarving, carvingDist);
if (mode == chisel_ros::ChiselServer::FusionMode::DepthImage)
{
server->SubscribeDepthImage(depthImageTopic, depthImageInfoTopic, depthImageTransform);
}
else
{
server->SubscribePointCloud(pointCloudTopic);
}
server->SetupDepthPosePublisher("last_depth_pose");
server->SetupDepthFrustumPublisher("last_depth_frustum");
server->SetNearPlaneDist(nearPlaneDist);
server->SetFarPlaneDist(farPlaneDist);
server->AdvertiseServices();
if (useColor && mode == chisel_ros::ChiselServer::FusionMode::DepthImage)
{
server->SubscribeColorImage(colorImageTopic, colorImageInfoTopic, colorImageTransform);
server->SetupColorPosePublisher("last_color_pose");
server->SetupColorFrustumPublisher("last_color_frustum");
}
server->SetBaseTransform(baseTransform);
server->SetupMeshPublisher(meshTopic);
server->SetupChunkBoxPublisher(chunkBoxTopic);
ROS_INFO("Beginning to loop.");
while (ros::ok())
{
ros::Rate loop_rate(100);
ros::spinOnce();
if(!server->IsPaused() && server->HasNewData())
{
ROS_INFO("Got data.");
switch (server->GetMode())
{
case chisel_ros::ChiselServer::FusionMode::DepthImage:
server->IntegrateLastDepthImage();
break;
case chisel_ros::ChiselServer::FusionMode::PointCloud:
server->IntegrateLastPointCloud();
break;
}
server->PublishMeshes();
server->PublishChunkBoxes();
if(mode == chisel_ros::ChiselServer::FusionMode::DepthImage)
{
server->PublishDepthPose();
server->PublishDepthFrustum();
if(useColor)
{
server->PublishColorPose();
server->PublishColorFrustum();
}
}
}
}
}
void SVGInlineTextBox::paint(PaintInfo& paintInfo, int, int)
{
ASSERT(paintInfo.shouldPaintWithinRoot(renderer()));
ASSERT(paintInfo.phase == PaintPhaseForeground || paintInfo.phase == PaintPhaseSelection);
ASSERT(truncation() == cNoTruncation);
if (renderer()->style()->visibility() != VISIBLE)
return;
// Note: We're explicitely not supporting composition & custom underlines and custom highlighters - unlike InlineTextBox.
// If we ever need that for SVG, it's very easy to refactor and reuse the code.
RenderObject* parentRenderer = parent()->renderer();
ASSERT(parentRenderer);
bool paintSelectedTextOnly = paintInfo.phase == PaintPhaseSelection;
bool hasSelection = !parentRenderer->document()->printing() && selectionState() != RenderObject::SelectionNone;
if (!hasSelection && paintSelectedTextOnly)
return;
RenderStyle* style = parentRenderer->style();
ASSERT(style);
const SVGRenderStyle* svgStyle = style->svgStyle();
ASSERT(svgStyle);
bool hasFill = svgStyle->hasFill();
bool hasStroke = svgStyle->hasStroke();
RenderStyle* selectionStyle = style;
if (hasSelection) {
selectionStyle = parentRenderer->getCachedPseudoStyle(SELECTION);
if (selectionStyle) {
const SVGRenderStyle* svgSelectionStyle = selectionStyle->svgStyle();
ASSERT(svgSelectionStyle);
if (!hasFill)
hasFill = svgSelectionStyle->hasFill();
if (!hasStroke)
hasStroke = svgSelectionStyle->hasStroke();
} else
selectionStyle = style;
}
unsigned textFragmentsSize = m_textFragments.size();
for (unsigned i = 0; i < textFragmentsSize; ++i) {
SVGTextFragment& fragment = m_textFragments.at(i);
ASSERT(!m_paintingResource);
paintInfo.context->save();
if (!fragment.transform.isIdentity())
paintInfo.context->concatCTM(fragment.transform);
// Spec: All text decorations except line-through should be drawn before the text is filled and stroked; thus, the text is rendered on top of these decorations.
int decorations = style->textDecorationsInEffect();
if (decorations & UNDERLINE)
paintDecoration(paintInfo.context, UNDERLINE, fragment);
if (decorations & OVERLINE)
paintDecoration(paintInfo.context, OVERLINE, fragment);
// Fill text
if (hasFill) {
m_paintingResourceMode = ApplyToFillMode | ApplyToTextMode;
paintText(paintInfo.context, style, selectionStyle, fragment, hasSelection, paintSelectedTextOnly);
}
// Stroke text
if (hasStroke) {
m_paintingResourceMode = ApplyToStrokeMode | ApplyToTextMode;
paintText(paintInfo.context, style, selectionStyle, fragment, hasSelection, paintSelectedTextOnly);
}
// Spec: Line-through should be drawn after the text is filled and stroked; thus, the line-through is rendered on top of the text.
if (decorations & LINE_THROUGH)
paintDecoration(paintInfo.context, LINE_THROUGH, fragment);
m_paintingResourceMode = ApplyToDefaultMode;
paintInfo.context->restore();
}
ASSERT(!m_paintingResource);
}
void SVGInlineTextBox::paintSelectionBackground(PaintInfo& paintInfo)
{
ASSERT(paintInfo.shouldPaintWithinRoot(renderer()));
ASSERT(paintInfo.phase == PaintPhaseForeground || paintInfo.phase == PaintPhaseSelection);
ASSERT(truncation() == cNoTruncation);
if (renderer()->style()->visibility() != VISIBLE)
return;
RenderObject* parentRenderer = parent()->renderer();
ASSERT(parentRenderer);
ASSERT(!parentRenderer->document()->printing());
// Determine whether or not we're selected.
bool paintSelectedTextOnly = paintInfo.phase == PaintPhaseSelection;
bool hasSelection = selectionState() != RenderObject::SelectionNone;
if (!hasSelection || paintSelectedTextOnly)
return;
Color backgroundColor = renderer()->selectionBackgroundColor();
if (!backgroundColor.isValid() || !backgroundColor.alpha())
return;
RenderStyle* style = parentRenderer->style();
ASSERT(style);
const SVGRenderStyle* svgStyle = style->svgStyle();
ASSERT(svgStyle);
bool hasFill = svgStyle->hasFill();
bool hasStroke = svgStyle->hasStroke();
RenderStyle* selectionStyle = style;
if (hasSelection) {
selectionStyle = parentRenderer->getCachedPseudoStyle(SELECTION);
if (selectionStyle) {
const SVGRenderStyle* svgSelectionStyle = selectionStyle->svgStyle();
ASSERT(svgSelectionStyle);
if (!hasFill)
hasFill = svgSelectionStyle->hasFill();
if (!hasStroke)
hasStroke = svgSelectionStyle->hasStroke();
} else
selectionStyle = style;
}
int startPosition, endPosition;
selectionStartEnd(startPosition, endPosition);
int fragmentStartPosition = 0;
int fragmentEndPosition = 0;
unsigned textFragmentsSize = m_textFragments.size();
for (unsigned i = 0; i < textFragmentsSize; ++i) {
SVGTextFragment& fragment = m_textFragments.at(i);
ASSERT(!m_paintingResource);
fragmentStartPosition = startPosition;
fragmentEndPosition = endPosition;
if (!mapStartEndPositionsIntoFragmentCoordinates(fragment, fragmentStartPosition, fragmentEndPosition))
continue;
paintInfo.context->save();
if (!fragment.transform.isIdentity())
paintInfo.context->concatCTM(fragment.transform);
paintInfo.context->setFillColor(backgroundColor, style->colorSpace());
paintInfo.context->fillRect(selectionRectForTextFragment(fragment, fragmentStartPosition, fragmentEndPosition, style), backgroundColor, style->colorSpace());
m_paintingResourceMode = ApplyToDefaultMode;
paintInfo.context->restore();
}
ASSERT(!m_paintingResource);
}
int run_main(int argc, char *argv[], int numprocs0, int myid0)
{
int MD_iter,i,j,po,ip;
char fileE[YOUSO10] = ".ene";
char fileDRC[YOUSO10] = ".md";
char fileMemory[YOUSO10];
char fileRestart[YOUSO10];
char operate[200];
double TStime,TEtime;
/* for idle CPUs */
int tag;
int complete;
MPI_Request request;
MPI_Status status;
/* for measuring elapsed time */
dtime(&TStime);
/* allocation of CompTime */
CompTime = (double**)malloc(sizeof(double*)*numprocs0);
for (i=0; i<numprocs0; i++){
CompTime[i] = (double*)malloc(sizeof(double)*30);
for (j=0; j<30; j++) CompTime[i][j] = 0.0;
}
if (myid0==Host_ID){
printf("\n*******************************************************\n");
printf("*******************************************************\n");
printf(" Welcome to OpenMX Ver. %s \n",Version_OpenMX);
printf(" Copyright (C), 2002-2009, T.Ozaki \n");
printf(" OpenMX comes with ABSOLUTELY NO WARRANTY. \n");
printf(" This is free software, and you are welcome to \n");
printf(" redistribute it under the constitution of the GNU-GPL.\n");
printf("*******************************************************\n");
printf("*******************************************************\n\n");
}
Init_List_YOUSO();
remake_headfile = 0;
ScaleSize = 1.2;
/****************************************************
Read the input file
****************************************************/
init_alloc_first();
CompTime[myid0][1] = readfile(argv);
MPI_Barrier(MPI_COMM_WORLD1);
/* initialize PrintMemory routine */
sprintf(fileMemory,"%s%s.memory%i",filepath,filename,myid0);
PrintMemory(fileMemory,0,"init");
PrintMemory_Fix();
/* initialize */
init();
fnjoint(filepath,filename,fileE);
fnjoint(filepath,filename,fileDRC);
/****************************************************
SCF-DFT calculations and MD and geometrical
optimization.
****************************************************/
MD_iter = 1;
do {
CompTime[myid0][2] += truncation(MD_iter,1);
if (ML_flag==1 && myid0==Host_ID) Get_VSZ(MD_iter);
if (Solver==4) {
TRAN_Calc_GridBound( mpi_comm_level1, atomnum, WhatSpecies, Spe_Atom_Cut1,
Ngrid1, Grid_Origin, Gxyz, tv, gtv, rgtv, Left_tv, Right_tv );
/* output: TRAN_region[], TRAN_grid_bound */
}
CompTime[myid0][3] += DFT(MD_iter,(MD_iter-1)%orbitalOpt_per_MDIter+1);
if (myid0==Host_ID) iterout(MD_iter,MD_TimeStep*MD_iter,fileE,fileDRC);
if (ML_flag==0) CompTime[myid0][4] += MD_pac(MD_iter,argv[1]);
MD_iter++;
} while(MD_Opt_OK==0 && MD_iter<=MD_IterNumber);
if ( TRAN_output_hks ) {
/* left is dummy */
TRAN_RestartFile(mpi_comm_level1, "write","left",filepath,TRAN_hksoutfilename);
}
/****************************************************
calculate Voronoi charge
****************************************************/
if (Voronoi_Charge_flag==1) Voronoi_Charge();
/****************************************************
making of a file *.frac for the fractional coordinates
****************************************************/
Make_FracCoord(argv[1]);
/****************************************************
generate Wannier functions added by Hongming Weng
****************************************************/
/* hmweng */
if(Wannier_Func_Calc){
if (myid0==Host_ID) printf("Calling Generate_Wannier...\n");fflush(0);
Generate_Wannier(argv[1]);
}
/****************************************************
Making of output files
****************************************************/
CompTime[myid0][20] = OutData(argv[1]);
/****************************************************
write connectivity, Hamiltonian, overlap, density
matrices, and etc. to a file, filename.scfout
****************************************************/
if (HS_fileout==1) SCF2File("write",argv[1]);
/* elapsed time */
dtime(&TEtime);
CompTime[myid0][0] = TEtime - TStime;
Output_CompTime();
for (i=0; i<numprocs0; i++){
free(CompTime[i]);
}
free(CompTime);
/* merge log files */
Merge_LogFile(argv[1]);
/* free arrays */
Free_Arrays(0);
/* print memory */
PrintMemory("total",0,"sum");
/****************************************************
reconstruct the original MPI group
****************************************************/
{
int *new_ranks;
MPI_Group new_group,old_group;
new_ranks = (int*)malloc(sizeof(int)*numprocs0);
for (i=0; i<numprocs0; i++) {
new_ranks[i]=i; /* a new group is made of original rank=0:Pnum[k]-1 */
}
MPI_Comm_group(MPI_COMM_WORLD1, &old_group);
/* define a new group */
MPI_Group_incl(old_group,numprocs0,new_ranks,&new_group);
MPI_Comm_create(MPI_COMM_WORLD1,new_group,&mpi_comm_level1);
MPI_Group_free(&new_group);
free(new_ranks); /* never forget cleaning! */
}
MPI_Barrier(MPI_COMM_WORLD1);
if (myid0==Host_ID){
printf("\nThe calculation was normally finished.\n");
}
return 0;
}
void SVGInlineTextBox::paint(PaintInfo& paintInfo, const LayoutPoint& paintOffset, LayoutUnit, LayoutUnit)
{
ASSERT(paintInfo.shouldPaintWithinRoot(renderer()));
ASSERT(paintInfo.phase == PaintPhaseForeground || paintInfo.phase == PaintPhaseSelection);
ASSERT(truncation() == cNoTruncation);
if (renderer().style().visibility() != VISIBLE)
return;
// Note: We're explicitely not supporting composition & custom underlines and custom highlighters - unlike InlineTextBox.
// If we ever need that for SVG, it's very easy to refactor and reuse the code.
auto& parentRenderer = parent()->renderer();
bool paintSelectedTextOnly = paintInfo.phase == PaintPhaseSelection;
bool hasSelection = !parentRenderer.document().printing() && selectionState() != RenderObject::SelectionNone;
if (!hasSelection && paintSelectedTextOnly)
return;
if (!textShouldBePainted(renderer()))
return;
RenderStyle& style = parentRenderer.style();
const SVGRenderStyle& svgStyle = style.svgStyle();
bool hasFill = svgStyle.hasFill();
bool hasVisibleStroke = svgStyle.hasVisibleStroke();
RenderStyle* selectionStyle = &style;
if (hasSelection) {
selectionStyle = parentRenderer.getCachedPseudoStyle(SELECTION);
if (selectionStyle) {
const SVGRenderStyle& svgSelectionStyle = selectionStyle->svgStyle();
if (!hasFill)
hasFill = svgSelectionStyle.hasFill();
if (!hasVisibleStroke)
hasVisibleStroke = svgSelectionStyle.hasVisibleStroke();
} else
selectionStyle = &style;
}
if (renderer().view().frameView().paintBehavior() & PaintBehaviorRenderingSVGMask) {
hasFill = true;
hasVisibleStroke = false;
}
AffineTransform fragmentTransform;
unsigned textFragmentsSize = m_textFragments.size();
for (unsigned i = 0; i < textFragmentsSize; ++i) {
SVGTextFragment& fragment = m_textFragments.at(i);
ASSERT(!m_paintingResource);
GraphicsContextStateSaver stateSaver(paintInfo.context());
fragment.buildFragmentTransform(fragmentTransform);
if (!fragmentTransform.isIdentity())
paintInfo.context().concatCTM(fragmentTransform);
// Spec: All text decorations except line-through should be drawn before the text is filled and stroked; thus, the text is rendered on top of these decorations.
int decorations = style.textDecorationsInEffect();
if (decorations & TextDecorationUnderline)
paintDecoration(paintInfo.context(), TextDecorationUnderline, fragment);
if (decorations & TextDecorationOverline)
paintDecoration(paintInfo.context(), TextDecorationOverline, fragment);
auto paintOrder = style.svgStyle().paintTypesForPaintOrder();
for (unsigned i = 0; i < paintOrder.size(); ++i) {
switch (paintOrder.at(i)) {
case PaintTypeFill:
if (!hasFill)
continue;
m_paintingResourceMode = ApplyToFillMode | ApplyToTextMode;
paintText(paintInfo.context(), &style, selectionStyle, fragment, hasSelection, paintSelectedTextOnly);
break;
case PaintTypeStroke:
if (!hasVisibleStroke)
continue;
m_paintingResourceMode = ApplyToStrokeMode | ApplyToTextMode;
paintText(paintInfo.context(), &style, selectionStyle, fragment, hasSelection, paintSelectedTextOnly);
break;
case PaintTypeMarkers:
continue;
}
}
// Spec: Line-through should be drawn after the text is filled and stroked; thus, the line-through is rendered on top of the text.
if (decorations & TextDecorationLineThrough)
paintDecoration(paintInfo.context(), TextDecorationLineThrough, fragment);
m_paintingResourceMode = ApplyToDefaultMode;
}
// Finally, paint the outline if any.
if (renderer().style().hasOutline() && is<RenderInline>(parentRenderer))
downcast<RenderInline>(parentRenderer).paintOutline(paintInfo, paintOffset);
ASSERT(!m_paintingResource);
}
REAL8 cdfwchisq(qfvars *vars, REAL8 sigma, REAL8 acc, INT4 *ifault)
{
INT4 nt, ntm;
REAL8 acc1, almx, xlim, xnt, xntm;
REAL8 utx, tausq, wnstd, intv, intv1, x, up, un, d1, d2;
REAL8 qfval;
INT4 rats[] = {1, 2, 4, 8};
*ifault = 0;
vars->count = 0;
vars->integrationValue = 0.0;
vars->integrationError = 0.0;
qfval = -1.0;
acc1 = acc;
vars->arrayNotSorted = 1;
vars->fail = 0;
xlim = (REAL8)vars->lim;
/* find wnmean, wnstd, wnmax and wnmin of weights, check that parameter values are valid */
vars->sigsq = sigma*sigma; //Sigma squared
wnstd = vars->sigsq; //weights*noise standard deviation initial value
vars->wnmax = 0.0; //Initial value for weights*noise maximum
vars->wnmin = 0.0; //Initial value for weights*noise minimum
vars->wnmean = 0.0; //Initial value for weights*noise 'mean'
for (UINT4 ii=0; ii<vars->weights->length; ii++ ) {
if ( vars->dofs->data[ii] < 0 || vars->noncentrality->data[ii] < 0.0 ) {
*ifault = 3;
return qfval;
} /* return error if any degrees of freedom is less than 0 or noncentrality parameter is less than 0.0 */
wnstd += vars->weights->data[ii]*vars->weights->data[ii] * (2 * vars->dofs->data[ii] + 4.0 * vars->noncentrality->data[ii]);
vars->wnmean += vars->weights->data[ii] * (vars->dofs->data[ii] + vars->noncentrality->data[ii]);
//Find maximum and minimum values
if (vars->wnmax < vars->weights->data[ii]) {
vars->wnmax = vars->weights->data[ii];
} else if (vars->wnmin > vars->weights->data[ii]) {
vars->wnmin = vars->weights->data[ii];
}
}
if ( wnstd == 0.0 ) {
if (vars->c>0.0) qfval = 1.0;
else qfval = 0.0;
return qfval;
}
if ( vars->wnmin == 0.0 && vars->wnmax == 0.0 && sigma == 0.0 ) {
*ifault = 3;
return qfval;
}
wnstd = sqrt(wnstd);
//almx is absolute value maximum of weights
if (vars->wnmax < -vars->wnmin) almx = -vars->wnmin;
else almx = vars->wnmax;
/* starting values for findu, cutoff */
utx = 16.0/wnstd;
up = 4.5/wnstd;
un = -up;
/* truncation point with no convergence factor */
findu(vars, &utx, 0.5*acc1);
/* does convergence factor help */
if (vars->c!=0.0 && almx>0.07*wnstd) {
tausq = 0.25*acc1/coeff(vars, vars->c);
if (vars->fail) vars->fail = 0;
else if (truncation(vars, utx, tausq) < 0.2*acc1) {
vars->sigsq += tausq;
findu(vars, &utx, 0.25*acc1);
}
}
acc1 *= 0.5;
/* find RANGE of distribution, quit if outside this */
l1:
d1 = cutoff(vars, acc1, &up) - vars->c;
if (d1 < 0.0) {
qfval = 1.0;
return qfval;
}
d2 = vars->c - cutoff(vars, acc1, &un);
if (d2 < 0.0) {
qfval = 0.0;
return qfval;
}
/* find integration interval */
if (d1>d2) intv = LAL_TWOPI/d1;
else intv = LAL_TWOPI/d2;
/* calculate number of terms required for main and auxillary integrations */
xnt = utx/intv;
xntm = 3.0/sqrt(acc1);
if (xnt>xntm*1.5) {
//parameters for auxillary integration
if (xntm>xlim) {
*ifault = 1;
return qfval;
}
ntm = (INT4)round(xntm);
intv1 = utx/ntm;
x = LAL_TWOPI/intv1;
if (x<=fabs(vars->c)) goto l2;
//calculate convergence factor
REAL8 coeffvalplusx = coeff(vars, vars->c+x);
REAL8 coeffvalminusx = coeff(vars, vars->c-x);
tausq = (1.0/3.0)*acc1/(1.1*(coeffvalminusx + coeffvalplusx));
if (vars->fail) goto l2;
acc1 = (2.0/3.0)*acc1;
//auxillary integration
//fprintf(stderr,"Num terms in auxillary integration %d\n", ntm);
integrate(vars, ntm, intv1, tausq, 0);
xlim -= xntm;
vars->sigsq += tausq;
//find truncation point with new convergence factor
findu(vars, &utx, 0.25*acc1);
acc1 *= 0.75;
goto l1;
}
/* main integration */
l2:
if (xnt > xlim) {
*ifault = 1;
return qfval;
}
nt = (INT4)round(xnt);
//fprintf(stderr,"Num terms in main integration %d\n", nt);
integrate(vars, nt, intv, 0.0, 1);
qfval = 0.5 - vars->integrationValue;
/* test whether round-off error could be significant allow for radix 8 or 16 machines */
up = vars->integrationError;
x = up + 0.1*acc;
for (UINT4 ii=0; ii<4; ii++) {
if (rats[ii] * x == rats[ii] * up) *ifault = 2;
}
return qfval;
} /* cdfwchisq() */
void Check_Force(char *argv[])
{
int ixyz;
int time1,time2,MD_iter;
double step;
double original_x,original_y,original_z;
double Analytic_Force[4];
double Numerical_Force[4];
double Utot1,Utot2,Utot3;
FILE *fp1;
char fname1[YOUSO10];
static int numprocs,myid;
/* MPI */
MPI_Comm_size(MPI_COMM_WORLD1,&numprocs);
MPI_Comm_rank(MPI_COMM_WORLD1,&myid);
/* initialize */
for (ixyz=0; ixyz<=3; ixyz++){
Analytic_Force[ixyz] = 0.0;
Numerical_Force[ixyz] = 0.0;
}
/* step for calculation of the numerical derivatives */
step = 0.0003;
/* flags */
F_Kin_flag = mbit[force_flag][0];
F_NL_flag = mbit[force_flag][1];
F_VNA_flag = mbit[force_flag][2];
F_dVHart_flag = mbit[force_flag][3];
F_Vxc_flag = mbit[force_flag][4];
F_VEF_flag = mbit[force_flag][5];
F_U_flag = mbit[force_flag][6];
/* store original coordinates */
original_x = Gxyz[1][1];
original_y = Gxyz[1][2];
original_z = Gxyz[1][3];
/* loop for ixyz */
for (ixyz=1; ixyz<=3; ixyz++){
Gxyz[1][1] = original_x;
Gxyz[1][2] = original_y;
Gxyz[1][3] = original_z;
MD_iter = 1;
time1 = truncation(MD_iter,1);
time2 = DFT(MD_iter,(MD_iter-1)%orbitalOpt_per_MDIter+1);
Utot1 = Utot;
/* analytic force */
if (myid==G2ID[1]){
Analytic_Force[ixyz] = -Gxyz[1][16+ixyz];
}
MPI_Bcast(&Analytic_Force[ixyz], 1, MPI_DOUBLE, G2ID[1], MPI_COMM_WORLD1);
/* do not use the restart files for later calculations */
Scf_RestartFromFile = 0;
MD_iter = 2;
Gxyz[1][ixyz] -= step;
time1 = truncation(MD_iter,1);
time2 = DFT(MD_iter,(MD_iter-1)%orbitalOpt_per_MDIter+1);
Utot2 = Utot;
/* do not use the restart files for later calculations */
Scf_RestartFromFile = 0;
MD_iter = 3;
Gxyz[1][ixyz] += 2.0*step;
time1 = truncation(MD_iter,1);
time2 = DFT(MD_iter,(MD_iter-1)%orbitalOpt_per_MDIter+1);
Utot3 = Utot;
/* numerical force */
if (myid==G2ID[1]){
Numerical_Force[ixyz] = -0.5*(Utot3 - Utot2)/step;
}
MPI_Bcast(&Numerical_Force[ixyz], 1, MPI_DOUBLE, G2ID[1], MPI_COMM_WORLD1);
}
/* save forces to a file */
if (myid==Host_ID){
sprintf(fname1,"forcetest.result");
if ( (fp1 = fopen(fname1,"a")) != NULL ){
fprintf(fp1,"\n");
fprintf(fp1,"%s\n",argv[1]);
fprintf(fp1," flag=%2d\n",force_flag);
fprintf(fp1," Numerical force= -(Utot(s+ds)-Utot(s-ds))/(2*ds)\n");
fprintf(fp1," ds=%15.10f\n",step);
fprintf(fp1," Forces (Hartree/Bohr) on atom 1\n");
fprintf(fp1," x y z\n");
fprintf(fp1," Analytic force %15.12f %15.12f %15.12f\n",
Analytic_Force[1],Analytic_Force[2],Analytic_Force[3]);
fprintf(fp1," Numerical force %15.12f %15.12f %15.12f\n",
Numerical_Force[1],Numerical_Force[2],Numerical_Force[3]);
fprintf(fp1," diff %15.12f %15.12f %15.12f\n",
Analytic_Force[1]-Numerical_Force[1],
Analytic_Force[2]-Numerical_Force[2],
Analytic_Force[3]-Numerical_Force[3]);
fclose(fp1);
}
else{
printf("Could not find %s in checking force consistency\n",fname1);
}
}
}