C++ rdgeom::Point3D类代码示例

void testIssue216() {
  RDNumeric::DoubleSymmMatrix dmat(4);
  dmat.setVal(0, 0, 0.0);
  dmat.setVal(0, 1, 1.0);
  dmat.setVal(0, 2, 1.0);
  dmat.setVal(0, 3, 1.0);
  dmat.setVal(1, 1, 0.0);
  dmat.setVal(1, 2, 1.0);
  dmat.setVal(1, 3, 1.0);
  dmat.setVal(2, 2, 0.0);
  dmat.setVal(2, 3, 1.0);
  dmat.setVal(3, 3, 0.0);

  std::cout << dmat;
  RDGeom::PointPtrVect pos;
  for (int i = 0; i < 4; i++) {
    RDGeom::Point3D *pt = new RDGeom::Point3D();
    pos.push_back(pt);
  }

  bool gotCoords = DistGeom::computeInitialCoords(dmat, pos);
  CHECK_INVARIANT(gotCoords, "");

  for (int i = 1; i < 4; i++) {
    RDGeom::Point3D pti = *(RDGeom::Point3D *)pos[i];
    for (int j = 0; j < i; j++) {
      RDGeom::Point3D ptj = *(RDGeom::Point3D *)pos[j];
      ptj -= pti;
      CHECK_INVARIANT(RDKit::feq(ptj.length(), 1.0, 0.02), "");
    }
  }
}

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

      double calculateCosY(const RDGeom::Point3D &iPoint,
        const RDGeom::Point3D &jPoint, const RDGeom::Point3D &kPoint,
        const RDGeom::Point3D &lPoint) {
        RDGeom::Point3D rJI = iPoint - jPoint;
        RDGeom::Point3D rJK = kPoint - jPoint;
        RDGeom::Point3D rJL = lPoint - jPoint;
        rJI /= rJI.length();
        rJK /= rJK.length();
        rJL /= rJL.length();

        RDGeom::Point3D n = rJI.crossProduct(rJK);
        n /= n.length();
        
        return n.dotProduct(rJL);
      }

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

double calcOopChi(const RDGeom::Point3D &iPoint, const RDGeom::Point3D &jPoint,
                  const RDGeom::Point3D &kPoint,
                  const RDGeom::Point3D &lPoint) {
  RDGeom::Point3D rJI = iPoint - jPoint;
  RDGeom::Point3D rJK = kPoint - jPoint;
  RDGeom::Point3D rJL = lPoint - jPoint;
  rJI /= rJI.length();
  rJK /= rJK.length();
  rJL /= rJL.length();

  RDGeom::Point3D n = rJI.crossProduct(rJK);
  n /= n.length();
  double sinChi = n.dotProduct(rJL);
  clipToOne(sinChi);

  return RAD2DEG * asin(sinChi);
}

    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 setBondLength(Conformer &conf,
                   unsigned int iAtomId, unsigned int jAtomId, double value) {
    RDGeom::POINT3D_VECT &pos = conf.getPositions();
    RANGE_CHECK(0, iAtomId, pos.size() - 1);
    RANGE_CHECK(0, jAtomId, pos.size() - 1);
    ROMol &mol = conf.getOwningMol();
    Bond *bond = mol.getBondBetweenAtoms(iAtomId, jAtomId);
    if(!bond) throw ValueErrorException("atoms i and j must be bonded");
    if(queryIsBondInRing(bond)) throw ValueErrorException("bond (i,j) must not belong to a ring");
    RDGeom::Point3D v = pos[iAtomId] - pos[jAtomId];
    double origValue = v.length();
    if(origValue <= 1.e-8) throw ValueErrorException("atoms i and j have identical 3D coordinates");

    // get all atoms bonded to j
    std::list<unsigned int> alist;
    _toBeMovedIdxList(mol, iAtomId, jAtomId, alist);
    v *= (value / origValue - 1.);
    for (std::list<unsigned int>::iterator it = alist.begin(); it != alist.end(); ++it) {
        pos[*it] -= v;
    }
}

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

    void InversionContrib::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 *g1 = &(grad[3 * d_at1Idx]);
      double *g2 = &(grad[3 * d_at2Idx]);
      double *g3 = &(grad[3 * d_at3Idx]);
      double *g4 = &(grad[3 * d_at4Idx]);

      RDGeom::Point3D rJI = p1 - p2;
      RDGeom::Point3D rJK = p3 - p2;
      RDGeom::Point3D rJL = p4 - p2;
      double dJI = rJI.length();
      double dJK = rJK.length();
      double dJL = rJL.length();
      if (isDoubleZero(dJI) || isDoubleZero(dJK) || isDoubleZero(dJL)) {
        return;
      }
      rJI /= dJI;
      rJK /= dJK;
      rJL /= dJL;
      
      RDGeom::Point3D n = (-rJI).crossProduct(rJK);
      n /= n.length();
      double cosY = n.dotProduct(rJL);
      double sinYSq = 1.0 - cosY * cosY;
      double sinY = std::max(((sinYSq > 0.0) ? sqrt(sinYSq) : 0.0), 1.0e-8);
      double cosTheta = rJI.dotProduct(rJK);
      double sinThetaSq = std::max(1.0 - cosTheta * cosTheta, 1.0e-8);
      double sinTheta = std::max(((sinThetaSq > 0.0) ? sqrt(sinThetaSq) : 0.0), 1.0e-8);
      // sin(2 * W) = 2 * sin(W) * cos(W) = 2 * cos(Y) * sin(Y)
      double dE_dW = -d_forceConstant
        * (d_C1 * cosY - 4.0 * d_C2 * cosY * sinY);
      RDGeom::Point3D t1 = rJL.crossProduct(rJK);
      RDGeom::Point3D t2 = rJI.crossProduct(rJL);
      RDGeom::Point3D t3 = rJK.crossProduct(rJI);
      double term1 = sinY * sinTheta;
      double term2 = cosY / (sinY * sinThetaSq);
      double tg1[3] = { (t1.x / term1 - (rJI.x - rJK.x * cosTheta) * term2) / dJI,
        (t1.y / term1 - (rJI.y - rJK.y * cosTheta) * term2) / dJI,
        (t1.z / term1 - (rJI.z - rJK.z * cosTheta) * term2) / dJI };
      double tg3[3] = { (t2.x / term1 - (rJK.x - rJI.x * cosTheta) * term2) / dJK,
        (t2.y / term1 - (rJK.y - rJI.y * cosTheta) * term2) / dJK,
        (t2.z / term1 - (rJK.z - rJI.z * cosTheta) * term2) / dJK };
      double tg4[3] = { (t3.x / term1 - rJL.x * cosY / sinY) / dJL,
        (t3.y / term1 - rJL.y * cosY / sinY) / dJL,
        (t3.z / term1 - rJL.z * cosY / sinY) / dJL };
      for (unsigned int i = 0; i < 3; ++i) {
        g1[i] += dE_dW * tg1[i];
        g2[i] += -dE_dW * (tg1[i] + tg3[i] + tg4[i]);
        g3[i] += dE_dW * tg3[i];
        g4[i] += dE_dW * tg4[i];
      }
    }

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 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 OopBendContrib::getGrad(double *pos, double *grad) const {
  PRECONDITION(dp_forceField, "no owner");
  PRECONDITION(pos, "bad vector");
  PRECONDITION(grad, "bad vector");

  RDGeom::Point3D iPoint(pos[3 * d_at1Idx], pos[3 * d_at1Idx + 1],
                         pos[3 * d_at1Idx + 2]);
  RDGeom::Point3D jPoint(pos[3 * d_at2Idx], pos[3 * d_at2Idx + 1],
                         pos[3 * d_at2Idx + 2]);
  RDGeom::Point3D kPoint(pos[3 * d_at3Idx], pos[3 * d_at3Idx + 1],
                         pos[3 * d_at3Idx + 2]);
  RDGeom::Point3D lPoint(pos[3 * d_at4Idx], pos[3 * d_at4Idx + 1],
                         pos[3 * d_at4Idx + 2]);
  double *g1 = &(grad[3 * d_at1Idx]);
  double *g2 = &(grad[3 * d_at2Idx]);
  double *g3 = &(grad[3 * d_at3Idx]);
  double *g4 = &(grad[3 * d_at4Idx]);

  RDGeom::Point3D rJI = iPoint - jPoint;
  RDGeom::Point3D rJK = kPoint - jPoint;
  RDGeom::Point3D rJL = lPoint - jPoint;
  double dJI = rJI.length();
  double dJK = rJK.length();
  double dJL = rJL.length();
  if (isDoubleZero(dJI) || isDoubleZero(dJK) || isDoubleZero(dJL)) {
    return;
  }
  rJI /= dJI;
  rJK /= dJK;
  rJL /= dJL;

  RDGeom::Point3D n = (-rJI).crossProduct(rJK);
  n /= n.length();
  double const c2 = MDYNE_A_TO_KCAL_MOL * DEG2RAD * DEG2RAD;
  double sinChi = rJL.dotProduct(n);
  clipToOne(sinChi);
  double cosChiSq = 1.0 - sinChi * sinChi;
  double cosChi = std::max(((cosChiSq > 0.0) ? sqrt(cosChiSq) : 0.0), 1.0e-8);
  double chi = RAD2DEG * asin(sinChi);
  double cosTheta = rJI.dotProduct(rJK);
  clipToOne(cosTheta);
  double sinThetaSq = std::max(1.0 - cosTheta * cosTheta, 1.0e-8);
  double sinTheta =
      std::max(((sinThetaSq > 0.0) ? sqrt(sinThetaSq) : 0.0), 1.0e-8);

  double dE_dChi = RAD2DEG * c2 * d_koop * chi;
  RDGeom::Point3D t1 = rJL.crossProduct(rJK);
  RDGeom::Point3D t2 = rJI.crossProduct(rJL);
  RDGeom::Point3D t3 = rJK.crossProduct(rJI);
  double term1 = cosChi * sinTheta;
  double term2 = sinChi / (cosChi * sinThetaSq);
  double tg1[3] = {(t1.x / term1 - (rJI.x - rJK.x * cosTheta) * term2) / dJI,
                   (t1.y / term1 - (rJI.y - rJK.y * cosTheta) * term2) / dJI,
                   (t1.z / term1 - (rJI.z - rJK.z * cosTheta) * term2) / dJI};
  double tg3[3] = {(t2.x / term1 - (rJK.x - rJI.x * cosTheta) * term2) / dJK,
                   (t2.y / term1 - (rJK.y - rJI.y * cosTheta) * term2) / dJK,
                   (t2.z / term1 - (rJK.z - rJI.z * cosTheta) * term2) / dJK};
  double tg4[3] = {(t3.x / term1 - rJL.x * sinChi / cosChi) / dJL,
                   (t3.y / term1 - rJL.y * sinChi / cosChi) / dJL,
                   (t3.z / term1 - rJL.z * sinChi / cosChi) / dJL};
  for (unsigned int i = 0; i < 3; ++i) {
    g1[i] += dE_dChi * tg1[i];
    g2[i] += -dE_dChi * (tg1[i] + tg3[i] + tg4[i]);
    g3[i] += dE_dChi * tg3[i];
    g4[i] += dE_dChi * tg4[i];
  }
}

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

bool comparePts(const RDGeom::Point3D &pt1, const RDGeom::Point3D &pt2, double tol=1.0e-4) {
  RDGeom::Point3D tpt = pt1;
  tpt -= pt2;
  return (tpt.length() < tol);
}