C++ Point3D::lengthSq方法代码示例

double getAngleRad(Conformer &conf,
                   unsigned int iAtomId, unsigned int jAtomId, unsigned int kAtomId) {
    RDGeom::POINT3D_VECT &pos = conf.getPositions();
    RANGE_CHECK(0, iAtomId, pos.size() - 1);
    RANGE_CHECK(0, jAtomId, pos.size() - 1);
    RANGE_CHECK(0, kAtomId, pos.size() - 1);
    RDGeom::Point3D rJI = pos[iAtomId] - pos[jAtomId];
    double rJISqLength = rJI.lengthSq();
    if(rJISqLength <= 1.e-16) throw ValueErrorException("atoms i and j have identical 3D coordinates");
    RDGeom::Point3D rJK = pos[kAtomId] - pos[jAtomId];
    double rJKSqLength = rJK.lengthSq();
    if(rJKSqLength <= 1.e-16) throw ValueErrorException("atoms j and k have identical 3D coordinates");
    return rJI.angleTo(rJK);
}

void setAngleRad(Conformer &conf, unsigned int iAtomId, unsigned int jAtomId,
                 unsigned int kAtomId, double value) {
  RDGeom::POINT3D_VECT &pos = conf.getPositions();
  URANGE_CHECK(iAtomId, pos.size());
  URANGE_CHECK(jAtomId, pos.size());
  URANGE_CHECK(kAtomId, pos.size());
  ROMol &mol = conf.getOwningMol();
  Bond *bondJI = mol.getBondBetweenAtoms(jAtomId, iAtomId);
  if (!bondJI) throw ValueErrorException("atoms i and j must be bonded");
  Bond *bondJK = mol.getBondBetweenAtoms(jAtomId, kAtomId);
  if (!bondJK) throw ValueErrorException("atoms j and k must be bonded");
  if (queryIsBondInRing(bondJI) && queryIsBondInRing(bondJK))
    throw ValueErrorException(
        "bonds (i,j) and (j,k) must not both belong to a ring");

  RDGeom::Point3D rJI = pos[iAtomId] - pos[jAtomId];
  double rJISqLength = rJI.lengthSq();
  if (rJISqLength <= 1.e-16)
    throw ValueErrorException("atoms i and j have identical 3D coordinates");
  RDGeom::Point3D rJK = pos[kAtomId] - pos[jAtomId];
  double rJKSqLength = rJK.lengthSq();
  if (rJKSqLength <= 1.e-16)
    throw ValueErrorException("atoms j and k have identical 3D coordinates");

  // we only need to rotate by delta with respect to the current angle value
  value -= rJI.angleTo(rJK);
  RDGeom::Point3D &rotAxisBegin = pos[jAtomId];
  // our rotation axis is the normal to the plane of atoms i, j, k
  RDGeom::Point3D rotAxisEnd = rJI.crossProduct(rJK) + pos[jAtomId];
  RDGeom::Point3D rotAxis = rotAxisEnd - rotAxisBegin;
  rotAxis.normalize();
  // get all atoms bonded to j and loop through them
  std::list<unsigned int> alist;
  _toBeMovedIdxList(mol, jAtomId, kAtomId, alist);
  for (std::list<unsigned int>::iterator it = alist.begin(); it != alist.end();
       ++it) {
    // translate atom so that it coincides with the origin of rotation
    pos[*it] -= rotAxisBegin;
    // rotate around our rotation axis
    RDGeom::Transform3D rotByAngle;
    rotByAngle.SetRotation(value, rotAxis);
    rotByAngle.TransformPoint(pos[*it]);
    // translate atom back
    pos[*it] += rotAxisBegin;
  }
}

double getDihedralRad(const Conformer &conf, unsigned int iAtomId,
                      unsigned int jAtomId, unsigned int kAtomId,
                      unsigned int lAtomId) {
  const RDGeom::POINT3D_VECT &pos = conf.getPositions();
  URANGE_CHECK(iAtomId, pos.size());
  URANGE_CHECK(jAtomId, pos.size());
  URANGE_CHECK(kAtomId, pos.size());
  URANGE_CHECK(lAtomId, pos.size());
  RDGeom::Point3D rIJ = pos[jAtomId] - pos[iAtomId];
  double rIJSqLength = rIJ.lengthSq();
  if (rIJSqLength <= 1.e-16)
    throw ValueErrorException("atoms i and j have identical 3D coordinates");
  RDGeom::Point3D rJK = pos[kAtomId] - pos[jAtomId];
  double rJKSqLength = rJK.lengthSq();
  if (rJKSqLength <= 1.e-16)
    throw ValueErrorException("atoms j and k have identical 3D coordinates");
  RDGeom::Point3D rKL = pos[lAtomId] - pos[kAtomId];
  double rKLSqLength = rKL.lengthSq();
  if (rKLSqLength <= 1.e-16)
    throw ValueErrorException("atoms k and l have identical 3D coordinates");

  RDGeom::Point3D nIJK = rIJ.crossProduct(rJK);
  double nIJKSqLength = nIJK.lengthSq();
  RDGeom::Point3D nJKL = rJK.crossProduct(rKL);
  double nJKLSqLength = nJKL.lengthSq();
  RDGeom::Point3D m = nIJK.crossProduct(rJK);
  // we want a signed dihedral, that's why we use atan2 instead of acos
  return -atan2(m.dotProduct(nJKL) / sqrt(nJKLSqLength * m.lengthSq()),
                nIJK.dotProduct(nJKL) / sqrt(nIJKSqLength * nJKLSqLength));
}

void setDihedralRad(Conformer &conf, unsigned int iAtomId, unsigned int jAtomId,
                    unsigned int kAtomId, unsigned int lAtomId, double value) {
  RDGeom::POINT3D_VECT &pos = conf.getPositions();
  URANGE_CHECK(iAtomId, pos.size());
  URANGE_CHECK(jAtomId, pos.size());
  URANGE_CHECK(kAtomId, pos.size());
  URANGE_CHECK(lAtomId, pos.size());
  ROMol &mol = conf.getOwningMol();
  Bond *bondIJ = mol.getBondBetweenAtoms(iAtomId, jAtomId);
  if (!bondIJ) throw ValueErrorException("atoms i and j must be bonded");
  Bond *bondJK = mol.getBondBetweenAtoms(jAtomId, kAtomId);
  if (!bondJK) throw ValueErrorException("atoms j and k must be bonded");
  Bond *bondKL = mol.getBondBetweenAtoms(kAtomId, lAtomId);
  if (!bondKL) throw ValueErrorException("atoms k and l must be bonded");

  if (queryIsBondInRing(bondJK))
    throw ValueErrorException("bond (j,k) must not belong to a ring");
  RDGeom::Point3D rIJ = pos[jAtomId] - pos[iAtomId];
  double rIJSqLength = rIJ.lengthSq();
  if (rIJSqLength <= 1.e-16)
    throw ValueErrorException("atoms i and j have identical 3D coordinates");
  RDGeom::Point3D rJK = pos[kAtomId] - pos[jAtomId];
  double rJKSqLength = rJK.lengthSq();
  if (rJKSqLength <= 1.e-16)
    throw ValueErrorException("atoms j and k have identical 3D coordinates");
  RDGeom::Point3D rKL = pos[lAtomId] - pos[kAtomId];
  double rKLSqLength = rKL.lengthSq();
  if (rKLSqLength <= 1.e-16)
    throw ValueErrorException("atoms k and l have identical 3D coordinates");

  RDGeom::Point3D nIJK = rIJ.crossProduct(rJK);
  double nIJKSqLength = nIJK.lengthSq();
  RDGeom::Point3D nJKL = rJK.crossProduct(rKL);
  double nJKLSqLength = nJKL.lengthSq();
  RDGeom::Point3D m = nIJK.crossProduct(rJK);
  // we only need to rotate by delta with respect to the current dihedral value
  value -= -atan2(m.dotProduct(nJKL) / sqrt(nJKLSqLength * m.lengthSq()),
                  nIJK.dotProduct(nJKL) / sqrt(nIJKSqLength * nJKLSqLength));
  // our rotation axis is the (j,k) bond
  RDGeom::Point3D &rotAxisBegin = pos[jAtomId];
  RDGeom::Point3D &rotAxisEnd = pos[kAtomId];
  RDGeom::Point3D rotAxis = rotAxisEnd - rotAxisBegin;
  rotAxis.normalize();
  // get all atoms bonded to k and loop through them
  std::list<unsigned int> alist;
  _toBeMovedIdxList(mol, jAtomId, kAtomId, alist);
  for (std::list<unsigned int>::iterator it = alist.begin(); it != alist.end();
       ++it) {
    // translate atom so that it coincides with the origin of rotation
    pos[*it] -= rotAxisBegin;
    // rotate around our rotation axis
    RDGeom::Transform3D rotByAngle;
    rotByAngle.SetRotation(value, rotAxis);
    rotByAngle.TransformPoint(pos[*it]);
    // translate atom back
    pos[*it] += rotAxisBegin;
  }
}

    double TorsionConstraintContrib::getEnergy(double *pos) const
    {
      PRECONDITION(dp_forceField, "no owner");
      PRECONDITION(pos, "bad vector");
 
      RDGeom::Point3D p1(pos[3 * d_at1Idx],
			  pos[3 * d_at1Idx + 1], pos[3 * d_at1Idx + 2]);
      RDGeom::Point3D p2(pos[3 * d_at2Idx],
			  pos[3 * d_at2Idx + 1], pos[3 * d_at2Idx + 2]);
      RDGeom::Point3D p3(pos[3 * d_at3Idx],
			  pos[3 * d_at3Idx + 1], pos[3 * d_at3Idx + 2]);
      RDGeom::Point3D p4(pos[3 * d_at4Idx],
			  pos[3 * d_at4Idx + 1], pos[3 * d_at4Idx + 2]);
      
      RDGeom::Point3D r1 = p1 - p2;
      RDGeom::Point3D r2 = p3 - p2;
      RDGeom::Point3D r3 = p2 - p3;
      RDGeom::Point3D r4 = p4 - p3;
      RDGeom::Point3D t1 = r1.crossProduct(r2);
      RDGeom::Point3D t2 = r3.crossProduct(r4);
      double d1 = std::max(t1.length(), 0.0);
      double d2 = std::max(t2.length(), 0.0);
      t1 /= d1;
      t2 /= d2;
      double cosPhi = t1.dotProduct(t2);
      
      RDGeom::Point3D n123 = (-r1).crossProduct(r2);
      double n123SqLength = n123.lengthSq();
      RDGeom::Point3D n234 = r2.crossProduct(r4);
      double n234SqLength = n234.lengthSq();
      RDGeom::Point3D m = n123.crossProduct(r2);
      // we want a signed dihedral, that's why we use atan2 instead of acos
      double dihedral = RAD2DEG * (-atan2(m.dotProduct(n234) / sqrt(n234SqLength * m.lengthSq()),
        n123.dotProduct(n234) / sqrt(n123SqLength * n234SqLength)));
      double ave = 0.5 * (d_minDihedralDeg + d_maxDihedralDeg);
      dihedral += 360.0 * boost::math::round((ave - dihedral) / 360.0);
      double dihedralTerm = 0.0;
      if (dihedral < d_minDihedralDeg) {
        dihedralTerm = dihedral - d_minDihedralDeg;
      }
      else if (dihedral > d_maxDihedralDeg) {
        dihedralTerm = dihedral - d_maxDihedralDeg;
      }
      double const c = 0.5 * DEG2RAD * DEG2RAD;
      double res = c * d_forceConstant * dihedralTerm * dihedralTerm;

      return res;
    }

    TorsionConstraintContrib::TorsionConstraintContrib(ForceField *owner,
      unsigned int idx1, unsigned int idx2, unsigned int idx3,
      unsigned int idx4, bool relative, double minDihedralDeg,
      double maxDihedralDeg, double forceConst)
    {
      PRECONDITION(owner,"bad owner");
      const RDGeom::PointPtrVect &pos = owner->positions();
      RANGE_CHECK(0, idx1, pos.size() - 1);
      RANGE_CHECK(0, idx2, pos.size() - 1);
      RANGE_CHECK(0, idx3, pos.size() - 1);
      RANGE_CHECK(0, idx4, pos.size() - 1);
      PRECONDITION((!(maxDihedralDeg < minDihedralDeg))
        && ((maxDihedralDeg - minDihedralDeg) < 360.0), "bad bounds");

      double dihedral = 0.0;
      if (relative) {
        RDGeom::Point3D p1 = *((RDGeom::Point3D *)pos[idx1]);
        RDGeom::Point3D p2 = *((RDGeom::Point3D *)pos[idx2]);
        RDGeom::Point3D p3 = *((RDGeom::Point3D *)pos[idx3]);
        RDGeom::Point3D p4 = *((RDGeom::Point3D *)pos[idx4]);
        RDGeom::Point3D r12 = p2 - p1;
        RDGeom::Point3D r23 = p3 - p2;
        RDGeom::Point3D r34 = p4 - p3;
        
        RDGeom::Point3D n123 = r12.crossProduct(r23);
        double nIJKSqLength = n123.lengthSq();
        RDGeom::Point3D n234 = r23.crossProduct(r34);
        double nJKLSqLength = n234.lengthSq();
        RDGeom::Point3D m = n123.crossProduct(r23);
        // we want a signed dihedral, that's why we use atan2 instead of acos
        dihedral = RAD2DEG
          * (-atan2(m.dotProduct(n234) / sqrt(nJKLSqLength * m.lengthSq()),
          n123.dotProduct(n234) / sqrt(nIJKSqLength * nJKLSqLength)));
      }
      dp_forceField = owner;
      d_at1Idx = idx1;
      d_at2Idx = idx2;
      d_at3Idx = idx3;
      d_at4Idx = idx4;
      minDihedralDeg += dihedral;
      maxDihedralDeg += dihedral;
      _pretreatDihedrals(minDihedralDeg, maxDihedralDeg);
      d_minDihedralDeg = minDihedralDeg;
      d_maxDihedralDeg = maxDihedralDeg;
      d_forceConstant = forceConst;
    }

    void TorsionConstraintContrib::getGrad(double *pos, double *grad) const
    {
      PRECONDITION(dp_forceField,"no owner");
      PRECONDITION(pos,"bad vector");
      PRECONDITION(grad,"bad vector");

      RDGeom::Point3D p1(pos[3 * d_at1Idx],
        pos[3 * d_at1Idx + 1], pos[3 * d_at1Idx + 2]);
      RDGeom::Point3D p2(pos[3 * d_at2Idx],
        pos[3 * d_at2Idx + 1], pos[3 * d_at2Idx + 2]);
      RDGeom::Point3D p3(pos[3 * d_at3Idx],
        pos[3 * d_at3Idx + 1], pos[3 * d_at3Idx + 2]);
      RDGeom::Point3D p4(pos[3 * d_at4Idx],
        pos[3 * d_at4Idx + 1], pos[3 * d_at4Idx + 2]);
      double *g[4] = {
        &(grad[3 * d_at1Idx]),
        &(grad[3 * d_at2Idx]),
        &(grad[3 * d_at3Idx]),
        &(grad[3 * d_at4Idx])
      };
      
      RDGeom::Point3D r[4] = {
        p1 - p2,
        p3 - p2,
        p2 - p3,
        p4 - p3
      };
      RDGeom::Point3D t[2] = {
        r[0].crossProduct(r[1]),
        r[2].crossProduct(r[3])
      };
      double d[2] = {
        t[0].length(),
        t[1].length()
      };
      if (isDoubleZero(d[0]) || isDoubleZero(d[1])) {
        return;
      }
      t[0] /= d[0];
      t[1] /= d[1];
      double cosPhi = t[0].dotProduct(t[1]);
      double sinPhiSq = 1.0 - cosPhi * cosPhi;
      double sinPhi = ((sinPhiSq > 0.0) ? sqrt(sinPhiSq) : 0.0);
      // dE/dPhi is independent of cartesians:
      
      RDGeom::Point3D n123 = (-r[0]).crossProduct(r[1]);
      double n123SqLength = n123.lengthSq();
      RDGeom::Point3D n234 = r[1].crossProduct(r[3]);
      double n234SqLength = n234.lengthSq();
      RDGeom::Point3D m = n123.crossProduct(r[1]);
      // we want a signed dihedral, that's why we use atan2 instead of acos
      double dihedral = RAD2DEG * (-atan2(m.dotProduct(n234) / sqrt(n234SqLength * m.lengthSq()),
        n123.dotProduct(n234) / sqrt(n123SqLength * n234SqLength)));
      //double dihedral = RAD2DEG * acos(cosPhi);
      double ave = 0.5 * (d_minDihedralDeg + d_maxDihedralDeg);
      dihedral += 360.0 * boost::math::round((ave - dihedral) / 360.0);
      double dihedralTerm = 0.0;
      if (dihedral < d_minDihedralDeg) {
        dihedralTerm = dihedral - d_minDihedralDeg;
      }
      else if (dihedral > d_maxDihedralDeg) {
        dihedralTerm = dihedral - d_maxDihedralDeg;
      }
      double dE_dPhi = DEG2RAD * d_forceConstant * dihedralTerm;
      
      // FIX: use a tolerance here
      // this is hacky, but it's per the
      // recommendation from Niketic and Rasmussen:
      double sinTerm = -dE_dPhi * (isDoubleZero(sinPhi)
        ? (1.0 / cosPhi) : (1.0 / sinPhi));
      Utils::calcTorsionGrad(r, t, d, g, sinTerm, cosPhi);
    }

double AlignPoints(const RDGeom::Point3DConstPtrVect &refPoints,
                   const RDGeom::Point3DConstPtrVect &probePoints,
                   RDGeom::Transform3D &trans, const DoubleVector *weights,
                   bool reflect, unsigned int maxIterations) {
  unsigned int npt = refPoints.size();
  PRECONDITION(npt == probePoints.size(), "Mismatch in number of points");
  trans.setToIdentity();
  const DoubleVector *wts;
  double wtsSum;
  bool ownWts;
  if (weights) {
    PRECONDITION(npt == weights->size(), "Mismatch in number of points");
    wts = weights;
    wtsSum = _sumOfWeights(*wts);
    ownWts = false;
  } else {
    wts = new DoubleVector(npt, 1.0);
    wtsSum = static_cast<double>(npt);
    ownWts = true;
  }

  RDGeom::Point3D rptSum = _weightedSumOfPoints(refPoints, *wts);
  RDGeom::Point3D pptSum = _weightedSumOfPoints(probePoints, *wts);

  double rptSumLenSq = _weightedSumOfLenSq(refPoints, *wts);
  double pptSumLenSq = _weightedSumOfLenSq(probePoints, *wts);

  double covMat[3][3];

  // compute the co-variance matrix
  _computeCovarianceMat(refPoints, probePoints, *wts, covMat);
  if (ownWts) {
    delete wts;
    wts = 0;
  }
  if (reflect) {
    rptSum *= -1.0;
    reflectCovMat(covMat);
  }

  // convert the covariance matrix to a 4x4 matrix that needs to be diagonalized
  double quad[4][4];
  _covertCovMatToQuad(covMat, rptSum, pptSum, wtsSum, quad);

  // get the eigenVecs and eigenVals for the matrix
  double eigenVecs[4][4], eigenVals[4];
  jacobi(quad, eigenVals, eigenVecs, maxIterations);

  // get the quaternion
  double quater[4];
  quater[0] = eigenVecs[0][0];
  quater[1] = eigenVecs[1][0];
  quater[2] = eigenVecs[2][0];
  quater[3] = eigenVecs[3][0];

  trans.SetRotationFromQuaternion(quater);
  if (reflect) {
    // put the flip in the rotation matrix
    trans.Reflect();
  }
  // compute the SSR value
  double ssr = eigenVals[0] - (pptSum.lengthSq() + rptSum.lengthSq()) / wtsSum +
               rptSumLenSq + pptSumLenSq;

  if ((ssr < 0.0) && (fabs(ssr) < TOLERANCE)) {
    ssr = 0.0;
  }
  if (reflect) {
    rptSum *= -1.0;
  }

  // set the translation
  trans.TransformPoint(pptSum);
  RDGeom::Point3D move = rptSum;
  move -= pptSum;
  move /= wtsSum;
  trans.SetTranslation(move);
  return ssr;
}