bool test(bool is_kernel_exact = true)
{
	// types
  typedef typename K::FT FT;
  typedef typename K::Line_3 Line;
  typedef typename K::Point_3 Point;
  typedef typename K::Segment_3 Segment;
  typedef typename K::Ray_3 Ray;
  typedef typename K::Line_3 Line;
  typedef typename K::Triangle_3 Triangle;

  /* -------------------------------------
  // Test data is something like that (in t supporting plane)
  // Triangle is (p1,p2,p3)
  //
  //       +E          +1
  //                 /   \
  //        +C     6+  +8  +4      +B
  //              /   9++7  \
  //            3+-------+5--+2
  //     
  //         +F        +A      
  ------------------------------------- */
  
  Point p1(FT(1.), FT(0.), FT(0.));
  Point p2(FT(0.), FT(1.), FT(0.));
  Point p3(FT(0.), FT(0.), FT(1.));
  
  Triangle t(p1,p2,p3);
  
  // Edges of t 
  Segment s12(p1,p2);
  Segment s21(p2,p1);
  Segment s13(p1,p3);
  Segment s23(p2,p3);
  Segment s32(p3,p2);
  Segment s31(p3,p1);
  
  bool b = test_aux(is_kernel_exact,t,s12,"t-s12",s12);
  b &= test_aux(is_kernel_exact,t,s21,"t-s21",s21);
  b &= test_aux(is_kernel_exact,t,s13,"t-s13",s13);
  b &= test_aux(is_kernel_exact,t,s23,"t-s23",s23);

  // Inside points
  Point p4(FT(0.5), FT(0.5), FT(0.));
  Point p5(FT(0.), FT(0.75), FT(0.25));
  Point p6(FT(0.5), FT(0.), FT(0.5));
  Point p7(FT(0.25), FT(0.625), FT(0.125));
  Point p8(FT(0.5), FT(0.25), FT(0.25));
  
  Segment s14(p1,p4);
  Segment s41(p4,p1);
  Segment s24(p2,p4);
  Segment s42(p4,p2);
  Segment s15(p1,p5);
  Segment s25(p2,p5);
  Segment s34(p3,p4);
  Segment s35(p3,p5);
  Segment s36(p3,p6);
  Segment s45(p4,p5);
  Segment s16(p1,p6);
  Segment s26(p2,p6);
  Segment s62(p6,p2);
  Segment s46(p4,p6);
  Segment s48(p4,p8);
  Segment s56(p5,p6);
  Segment s65(p6,p5);
  Segment s64(p6,p4);
  Segment s17(p1,p7);
  Segment s67(p6,p7);
  Segment s68(p6,p8);
  Segment s86(p8,p6);
  Segment s78(p7,p8);
  Segment s87(p8,p7);
  
  b &= test_aux(is_kernel_exact,t,s14,"t-s14",s14);
  b &= test_aux(is_kernel_exact,t,s41,"t-s41",s41);
  b &= test_aux(is_kernel_exact,t,s24,"t-s24",s24);
  b &= test_aux(is_kernel_exact,t,s42,"t-s42",s42);
  b &= test_aux(is_kernel_exact,t,s15,"t-s15",s15);
  b &= test_aux(is_kernel_exact,t,s25,"t-s25",s25);
  b &= test_aux(is_kernel_exact,t,s34,"t-s34",s34);
  b &= test_aux(is_kernel_exact,t,s35,"t-s35",s35);
  b &= test_aux(is_kernel_exact,t,s36,"t-s36",s36);
  b &= test_aux(is_kernel_exact,t,s45,"t-s45",s45);
  b &= test_aux(is_kernel_exact,t,s16,"t-s16",s16);
  b &= test_aux(is_kernel_exact,t,s26,"t-s26",s26);
  b &= test_aux(is_kernel_exact,t,s62,"t-s62",s62);
  b &= test_aux(is_kernel_exact,t,s46,"t-s46",s46);
  b &= test_aux(is_kernel_exact,t,s65,"t-s65",s65);
  b &= test_aux(is_kernel_exact,t,s64,"t-s64",s64);
  b &= test_aux(is_kernel_exact,t,s48,"t-s48",s48);
  b &= test_aux(is_kernel_exact,t,s56,"t-s56",s56);
  b &= test_aux(is_kernel_exact,t,s17,"t-t17",s17);
  b &= test_aux(is_kernel_exact,t,s67,"t-t67",s67);
  b &= test_aux(is_kernel_exact,t,s68,"t-s68",s68);
  b &= test_aux(is_kernel_exact,t,s86,"t-s86",s86);
  b &= test_aux(is_kernel_exact,t,s78,"t-t78",s78);
  b &= test_aux(is_kernel_exact,t,s87,"t-t87",s87);
  
  // Outside points (in triangle plane)
  Point pA(FT(-0.5), FT(1.), FT(0.5));
  Point pB(FT(0.5), FT(1.), FT(-0.5));
  Point pC(FT(0.5), FT(-0.5), FT(1.));
  Point pE(FT(1.), FT(-1.), FT(1.));
  Point pF(FT(-1.), FT(0.), FT(2.));
  
  Segment sAB(pA,pB);
  Segment sBC(pB,pC);
  Segment s2E(p2,pE);
  Segment sE2(pE,p2);
  Segment s2A(p2,pA);
  Segment s6E(p6,pE);
  Segment sB8(pB,p8);
  Segment sC8(pC,p8);
  Segment s8C(p8,pC);
  Segment s1F(p1,pF);
  Segment sF6(pF,p6);
  
  b &= test_aux(is_kernel_exact,t,sAB,"t-sAB",p2);
  b &= test_aux(is_kernel_exact,t,sBC,"t-sBC",s46);
  b &= test_aux(is_kernel_exact,t,s2E,"t-s2E",s26);
  b &= test_aux(is_kernel_exact,t,sE2,"t-sE2",s62);
  b &= test_aux(is_kernel_exact,t,s2A,"t-s2A",p2);
  b &= test_aux(is_kernel_exact,t,s6E,"t-s6E",p6);
  b &= test_aux(is_kernel_exact,t,sB8,"t-sB8",s48);
  b &= test_aux(is_kernel_exact,t,sC8,"t-sC8",s68);
  b &= test_aux(is_kernel_exact,t,s8C,"t-s8C",s86);
  b &= test_aux(is_kernel_exact,t,s1F,"t-s1F",s13);
  b &= test_aux(is_kernel_exact,t,sF6,"t-sF6",s36);
  
  // Outside triangle plane
  Point pa(FT(0.), FT(0.), FT(0.));
  Point pb(FT(2.), FT(0.), FT(0.));
  Point pc(FT(1.), FT(0.), FT(1.));
  Point pe(FT(1.), FT(0.5), FT(0.5));
  
  Segment sab(pa,pb);
  Segment sac(pa,pc);
  Segment sae(pa,pe);
  Segment sa8(pa,p8);
  Segment sb2(pb,p2);
  
  b &= test_aux(is_kernel_exact,t,sab,"t-sab",p1);
  b &= test_aux(is_kernel_exact,t,sac,"t-sac",p6);
  b &= test_aux(is_kernel_exact,t,sae,"t-sae",p8);
  b &= test_aux(is_kernel_exact,t,sa8,"t-sa8",p8);
  b &= test_aux(is_kernel_exact,t,sb2,"t-sb2",p2);
  
  // -----------------------------------
  // ray queries
  // -----------------------------------
  // Edges of t 
  Ray r12(p1,p2);
  Ray r21(p2,p1);
  Ray r13(p1,p3);
  Ray r23(p2,p3);
  
  b &= test_aux(is_kernel_exact,t,r12,"t-r12",s12);
  b &= test_aux(is_kernel_exact,t,r21,"t-r21",s21);
  b &= test_aux(is_kernel_exact,t,r13,"t-r13",s13);
  b &= test_aux(is_kernel_exact,t,r23,"t-r23",s23);
  
  // In triangle
  Point p9_(FT(0.), FT(0.5), FT(0.5));
  Point p9(FT(0.25), FT(0.375), FT(0.375));
  
  Ray r14(p1,p4);
  Ray r41(p4,p1);
  Ray r24(p2,p4);
  Ray r42(p4,p2);
  Ray r15(p1,p5);
  Ray r25(p2,p5);
  Ray r34(p3,p4);
  Ray r35(p3,p5);
  Ray r36(p3,p6);
  Ray r45(p4,p5);
  Ray r16(p1,p6);
  Ray r26(p2,p6);
  Ray r62(p6,p2);
  Ray r46(p4,p6);
  Ray r48(p4,p8);
  Ray r56(p5,p6);
  Ray r47(p4,p7);
  Ray r89(p8,p9);
  Ray r86(p8,p6);
  Ray r68(p6,p8);
  Segment r89_res(p8,p9_);
  
  b &= test_aux(is_kernel_exact,t,r14,"t-r14",s12);
  b &= test_aux(is_kernel_exact,t,r41,"t-r41",s41);
  b &= test_aux(is_kernel_exact,t,r24,"t-r24",s21);
  b &= test_aux(is_kernel_exact,t,r42,"t-r42",s42);
  b &= test_aux(is_kernel_exact,t,r15,"t-r15",s15);
  b &= test_aux(is_kernel_exact,t,r25,"t-r25",s23);
  b &= test_aux(is_kernel_exact,t,r34,"t-r34",s34);
  b &= test_aux(is_kernel_exact,t,r35,"t-r35",s32);
  b &= test_aux(is_kernel_exact,t,r36,"t-r36",s31);
  b &= test_aux(is_kernel_exact,t,r45,"t-r45",s45);
  b &= test_aux(is_kernel_exact,t,r16,"t-r16",s13);
  b &= test_aux(is_kernel_exact,t,r26,"t-r26",s26);
  b &= test_aux(is_kernel_exact,t,r62,"t-r62",s62);
  b &= test_aux(is_kernel_exact,t,r46,"t-r46",s46);
  b &= test_aux(is_kernel_exact,t,r48,"t-r48",s46);
  b &= test_aux(is_kernel_exact,t,r56,"t-r56",s56);
  b &= test_aux(is_kernel_exact,t,r47,"t-r47",s45);
  b &= test_aux(is_kernel_exact,t,r89,"t-t89",r89_res);
  b &= test_aux(is_kernel_exact,t,r68,"t-r68",s64);
  b &= test_aux(is_kernel_exact,t,r86,"t-r86",s86);
  
  
  // Outside points (in triangre prane)
  Ray rAB(pA,pB);
  Ray rBC(pB,pC);
  Ray r2E(p2,pE);
  Ray rE2(pE,p2);
  Ray r2A(p2,pA);
  Ray r6E(p6,pE);
  Ray rB8(pB,p8);
  Ray rC8(pC,p8);
  Ray r8C(p8,pC);
  Ray r1F(p1,pF);
  Ray rF6(pF,p6);
  
  b &= test_aux(is_kernel_exact,t,rAB,"t-rAB",p2);
  b &= test_aux(is_kernel_exact,t,rBC,"t-rBC",s46);
  b &= test_aux(is_kernel_exact,t,r2E,"t-r2E",s26);
  b &= test_aux(is_kernel_exact,t,rE2,"t-rE2",s62);
  b &= test_aux(is_kernel_exact,t,r2A,"t-r2A",p2);
  b &= test_aux(is_kernel_exact,t,r6E,"t-r6E",p6);
  b &= test_aux(is_kernel_exact,t,rB8,"t-rB8",s46);
  b &= test_aux(is_kernel_exact,t,rC8,"t-rC8",s64);
  b &= test_aux(is_kernel_exact,t,r8C,"t-r8C",s86);
  b &= test_aux(is_kernel_exact,t,r1F,"t-r1F",s13);
  b &= test_aux(is_kernel_exact,t,rF6,"t-rF6",s31);
  
  // Outside triangle plane
  Ray rab(pa,pb);
  Ray rac(pa,pc);
  Ray rae(pa,pe);
  Ray ra8(pa,p8);
  Ray rb2(pb,p2);
  
  b &= test_aux(is_kernel_exact,t,rab,"t-rab",p1);
  b &= test_aux(is_kernel_exact,t,rac,"t-rac",p6);
  b &= test_aux(is_kernel_exact,t,rae,"t-rae",p8);
  b &= test_aux(is_kernel_exact,t,ra8,"t-ra8",p8);
  b &= test_aux(is_kernel_exact,t,rb2,"t-rb2",p2);
  
  // -----------------------------------
  // Line queries
  // -----------------------------------
  // Edges of t 
  Line l12(p1,p2);
  Line l21(p2,p1);
  Line l13(p1,p3);
  Line l23(p2,p3);
  
  b &= test_aux(is_kernel_exact,t,l12,"t-l12",s12);
  b &= test_aux(is_kernel_exact,t,l21,"t-l21",s21);
  b &= test_aux(is_kernel_exact,t,l13,"t-l13",s13);
  b &= test_aux(is_kernel_exact,t,l23,"t-l23",s23);
  
  // In triangle
  Line l14(p1,p4);
  Line l41(p4,p1);
  Line l24(p2,p4);
  Line l42(p4,p2);
  Line l15(p1,p5);
  Line l25(p2,p5);
  Line l34(p3,p4);
  Line l35(p3,p5);
  Line l36(p3,p6);
  Line l45(p4,p5);
  Line l16(p1,p6);
  Line l26(p2,p6);
  Line l62(p6,p2);
  Line l46(p4,p6);
  Line l48(p4,p8);
  Line l56(p5,p6);
  Line l47(p4,p7);
  Line l89(p8,p9);
  Line l86(p8,p6);
  Line l68(p6,p8);
  Segment l89_res(p1,p9_);

  
  b &= test_aux(is_kernel_exact,t,l14,"t-l14",s12);
  b &= test_aux(is_kernel_exact,t,l41,"t-l41",s21);
  b &= test_aux(is_kernel_exact,t,l24,"t-l24",s21);
  b &= test_aux(is_kernel_exact,t,l42,"t-l42",s12);
  b &= test_aux(is_kernel_exact,t,l15,"t-l15",s15);
  b &= test_aux(is_kernel_exact,t,l25,"t-l25",s23);
  b &= test_aux(is_kernel_exact,t,l34,"t-l34",s34);
  b &= test_aux(is_kernel_exact,t,l35,"t-l35",s32);
  b &= test_aux(is_kernel_exact,t,l36,"t-l36",s31);
  b &= test_aux(is_kernel_exact,t,l45,"t-l45",s45);
  b &= test_aux(is_kernel_exact,t,l16,"t-l16",s13);
  b &= test_aux(is_kernel_exact,t,l26,"t-l26",s26);
  b &= test_aux(is_kernel_exact,t,l62,"t-l62",s62);
  b &= test_aux(is_kernel_exact,t,l46,"t-l46",s46);
  b &= test_aux(is_kernel_exact,t,l48,"t-l48",s46);
  b &= test_aux(is_kernel_exact,t,l56,"t-l56",s56);
  b &= test_aux(is_kernel_exact,t,l47,"t-l47",s45);
  b &= test_aux(is_kernel_exact,t,l89,"t-t89",l89_res);
  b &= test_aux(is_kernel_exact,t,l68,"t-l68",s64);
  b &= test_aux(is_kernel_exact,t,l86,"t-l86",s46);

  
  // Outside points (in triangle plane)
  Line lAB(pA,pB);
  Line lBC(pB,pC);
  Line l2E(p2,pE);
  Line lE2(pE,p2);
  Line l2A(p2,pA);
  Line l6E(p6,pE);
  Line lB8(pB,p8);
  Line lC8(pC,p8);
  Line l8C(p8,pC);
  Line l1F(p1,pF);
  Line lF6(pF,p6);
  
  b &= test_aux(is_kernel_exact,t,lAB,"t-lAB",p2);
  b &= test_aux(is_kernel_exact,t,lBC,"t-lBC",s46);
  b &= test_aux(is_kernel_exact,t,l2E,"t-l2E",s26);
  b &= test_aux(is_kernel_exact,t,lE2,"t-lE2",s62);
  b &= test_aux(is_kernel_exact,t,l2A,"t-l2A",p2);
  b &= test_aux(is_kernel_exact,t,l6E,"t-l6E",s26);
  b &= test_aux(is_kernel_exact,t,lB8,"t-lB8",s46);
  b &= test_aux(is_kernel_exact,t,lC8,"t-lC8",s64);
  b &= test_aux(is_kernel_exact,t,l8C,"t-l8C",s46);
  b &= test_aux(is_kernel_exact,t,l1F,"t-l1F",s13);
  b &= test_aux(is_kernel_exact,t,lF6,"t-lF6",s31);
  
  // Outside triangle plane
  Line lab(pa,pb);
  Line lac(pa,pc);
  Line lae(pa,pe);
  Line la8(pa,p8);
  Line lb2(pb,p2);
  
  b &= test_aux(is_kernel_exact,t,lab,"t-lab",p1);
  b &= test_aux(is_kernel_exact,t,lac,"t-lac",p6);
  b &= test_aux(is_kernel_exact,t,lae,"t-lae",p8);
  b &= test_aux(is_kernel_exact,t,la8,"t-la8",p8);
  b &= test_aux(is_kernel_exact,t,lb2,"t-lb2",p2);
  
  
	return b;
}
Example #2
0
/**
 * Get the Wilson's B-matrix
 */
void getBMatrix(Real** cartCoords, int numCartesians,
		bondCoord** bonds, int numBonds,
		angleCoord** angles, int numAngles,
		dihedralCoord** dihedrals, int numDihedrals,
		improperCoord** impropers, int numImpropers,
		Matrix& B) {
#ifdef DEBUG
  cout << "\n\ngetBMatrix - Constructing B Matrix\n";
#endif

  // Constructing B Matrix
  //   follows method in chapter 4 of Molecular Vibrations by Wilson, Decius, and Cross

#ifdef DEBUG  
  int numInternals = numBonds + numAngles + numDihedrals + numImpropers;
  cout << "numBonds: " << numBonds << "\n";
  cout << "numAngles: " << numAngles << "\n";
  cout << "numDihedrals: " << numDihedrals << "\n";
  cout << "numImpropers: " << numImpropers << "\n";
  cout << "numInternals: " << numInternals << "\n";
#endif

  // Load Data
  B = 0.0;
  int i = 0;
  int j = 0;
  int index1 = 0;
  int index2 = 0;
  RowVector tempCoord1(3);
  RowVector tempCoord2(3);
  Real norm = 0.0;
  
  // Bonds
  for (i=0; i<numBonds; i++) {
    index1 = bonds[i]->x1;
    index2 = bonds[i]->x2;
    //norm = bonds[i].val;   // Could calculate this, like below.
#ifdef DEBUG
    cout << "index1=" << index1 << "index2=" << index2 << "\n";
#endif
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
    }
    tempCoord1 << tempCoord1 - tempCoord2;
    norm = tempCoord1.NormFrobenius();   // XXX - don't delete
    if (norm > 0.0) {
      tempCoord1 << tempCoord1 / norm;
    }
    for (j=1; j<=3; j++) {
      B(i+1,((index1)*3)+j) =  tempCoord1(j);
      B(i+1,((index2)*3)+j) = -tempCoord1(j);
    }
  }
  
#ifdef DEBUG
  cout << "after bonds\n";
  cout << "B:\n";
  cout << setw(9) << setprecision(3) << (B);
  cout << "\n\n";
#endif

  // Angles
  int index3 = 0;
  RowVector tempCoord3(3);
  RowVector tempCoord4(3);
  RowVector tempCoord5(3);
  RowVector r21(3);   // Vector from 2nd to 1st point
  RowVector r23(3);   // Vector from 2nd to 3rd point
  RowVector e21(3);   // Unit vector from 2nd to 1st point
  RowVector e23(3);   // Unit vector from 2nd to 3rd point
  Real norm21;        // Norm of r21
  Real norm23;        // Norm of r23
  Real angle = 0.0;   // Angle in radians
  Real cosAngle123 = 0.0;
  Real sinAngle123 = 0.0;
  //Real pi = 3.14159265;
  Real scaleFactor = 0.529178;   // Scaling factor (0.529178)
  for (i=0; i<numAngles; i++) {
    index1 = angles[i]->x1;
    index2 = angles[i]->x2;
    index3 = angles[i]->x3;
    //angle = angles[i].val * (pi/180.0);   // Convert to radians.
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
      tempCoord3(j+1) = cartCoords[index3][j];
    }
    r21 << tempCoord1 - tempCoord2;
    r23 << tempCoord3 - tempCoord2;
    norm21 = r21.NormFrobenius();
    norm23 = r23.NormFrobenius();
    e21 << r21;
    if (norm21 > 0.0) {
      e21 << e21 / norm21;
    }
    e23 << r23;
    if (norm23 > 0.0) {
      e23 << e23 / norm23;
    }
    angle = acos(DotProduct(r21,r23) / (norm21 * norm23));
    cosAngle123 = DotProduct(r21,r23) / (norm21 * norm23);
    sinAngle123 = sqrt(1 - (cosAngle123 * cosAngle123));
    
#ifdef DEBUG
    cout << "r21: " << (r21) << "\n";
    cout << "r23: " << (r23) << "\n";
    cout << "norm21: " << norm21 << ", norm23: " << norm23 << "\n\n";
    cout << "e21: " << (e21) << "\n";
    cout << "e23: " << (e23) << "\n";
    cout << "cos(" << angle << "): " << cos(angle) << "\n";
    cout << "sin(" << angle << "): " << sin(angle) << "\n";
    cout << "angle: " << acos(DotProduct(r21,r23) / (norm21 * norm23)) << "\n";
    cout << "cosAngle123: " << cosAngle123 << "\n";
    cout << "sinAngle123: " << sinAngle123 << "\n";
#endif    

    // First elements of coordinate triples
    tempCoord4 << ((cosAngle123 * e21) - e23);
    tempCoord4 << (tempCoord4 * scaleFactor) / (norm21 * sinAngle123);
    for (j=1; j<=3; j++) {
      B(i+numBonds+1,((index1)*3)+j) = tempCoord4(j);
    }
    // Third elements of coordinate triples
    tempCoord5 << ((cosAngle123 * e23) - e21);
    tempCoord5 << (tempCoord5 * scaleFactor) / (norm23 * sinAngle123);
    for (j=1; j<=3; j++) {
      B(i+numBonds+1,((index3)*3)+j) = tempCoord5(j);
    }
    // Second (middle) elements of coordinate triples (depends on 1st and 3rd)
    tempCoord4 << -tempCoord4 - tempCoord5;
    for (j=1; j<=3; j++) {
      B(i+numBonds+1,((index2)*3)+j) = tempCoord4(j);
    }
  }
  
#ifdef DEBUG
  cout << "after angles\n";
  cout << "B:\n";
  cout << setw(9) << setprecision(3) << (B);
  cout << "\n\n";
#endif  

  // Dihedrals
  RowVector r12(3);   // Vector from 1st to 2nd point
  RowVector r32(3);   // Vector from 3rd to 2nd point
  RowVector r34(3);   // Vector from 3rd to 2nd point
  RowVector r43(3);   // Vector from 4th to 3rd point
  RowVector e12(3);   // Unit vector from 1st to 2nd point
  RowVector e32(3);   // Unit vector from 3rd to 2nd point
  RowVector e34(3);   // Unit vector from 3rd to 2nd point
  RowVector e43(3);   // Unit vector from 4th to 3rd point
  Real norm12;        // Norm of r12
  Real norm32;        // Norm of r32
  Real norm34;        // Norm of r34
  Real norm43;        // Norm of r43
  RowVector cross1223(3);   // Cross product of e12 and e23
  RowVector cross4332(3);   // Cross product of e43 and e32
  Real angle123 = 0.0;   // Angle in radians
  Real angle234 = 0.0;   // Angle in radians
  Real cosAngle234 = 0.0;
  Real sinAngle234 = 0.0;
  scaleFactor = 0.529178;   // Scaling factor (0.529178)
  int index4 = 0;
  RowVector tempCoord6(3);
  for (i=0; i<numDihedrals; i++) {
    index1 = dihedrals[i]->x1;
    index2 = dihedrals[i]->x2;
    index3 = dihedrals[i]->x3;
    index4 = dihedrals[i]->x4;
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
      tempCoord3(j+1) = cartCoords[index3][j];
      tempCoord4(j+1) = cartCoords[index4][j];
    }
    r12 << tempCoord2 - tempCoord1;
    r21 << tempCoord1 - tempCoord2;
    r23 << tempCoord3 - tempCoord2;
    r32 << tempCoord2 - tempCoord3;
    r34 << tempCoord4 - tempCoord3;
    r43 << tempCoord3 - tempCoord4;
    norm12 = r12.NormFrobenius();
    norm21 = r21.NormFrobenius();
    norm23 = r23.NormFrobenius();
    norm32 = r32.NormFrobenius();
    norm34 = r34.NormFrobenius();
    norm43 = r43.NormFrobenius();
#ifdef DEBUG
    cout << "norm12: " << norm12 << "\n";
    cout << "norm21: " << norm21 << "\n";
    cout << "norm23: " << norm23 << "\n";
    cout << "norm32: " << norm32 << "\n";
    cout << "norm34: " << norm34 << "\n";
    cout << "norm43: " << norm43 << "\n";
#endif
    e12 << r12 / norm12;
    e21 << r21 / norm21;
    e23 << r23 / norm23;
    e32 << r32 / norm32;
    e34 << r34 / norm34;
    e43 << r43 / norm43;
    angle123 = acos(DotProduct(r21,r23) / (norm21 * norm23));   // Wilson's angle 2
    angle234 = acos(DotProduct(r32,r34) / (norm32 * norm34));   // Wilson's angle 3
    cosAngle123 = DotProduct(r21,r23) / (norm21 * norm23);
    cosAngle234 = DotProduct(r32,r34) / (norm32 * norm34);
    sinAngle123 = sqrt(1 - (cosAngle123 * cosAngle123));
    sinAngle234 = sqrt(1 - (cosAngle234 * cosAngle234));
#ifdef DEBUG
    cout << "angle123: " << angle123 << ", cos(angle123): " << cos(angle123) << ", sin(angle123): " << sin(angle123) << "\n";
    cout << "angle234: " << angle234 << ", cos(angle234): " << cos(angle234) << ", sin(angle234): " << sin(angle234) << "\n";
    cout << "cosAngle123: " << cosAngle123 << ", sinAngle123: " << sinAngle123 << "\n";
    cout << "cosAngle234: " << cosAngle234 << ", sinAngle234: " << sinAngle234 << "\n";
#endif
    cross1223(1) = (e12(2)*e23(3)) - (e12(3)*e23(2));
    cross1223(2) = (e12(3)*e23(1)) - (e12(1)*e23(3));
    cross1223(3) = (e12(1)*e23(2)) - (e12(2)*e23(1));
    cross4332(1) = (e43(2)*e32(3)) - (e43(3)*e32(2));
    cross4332(2) = (e43(3)*e32(1)) - (e43(1)*e32(3));
    cross4332(3) = (e43(1)*e32(2)) - (e43(2)*e32(1));
#ifdef DEBUG
    cout << "cross1223 (norm " << cross1223.NormFrobenius() << "):\n";
    cout << setw(9) << setprecision(6) << (cross1223);
    cout << "\n\n";
    cout << "cross4332 (norm " << cross4332.NormFrobenius() << "):\n";
    cout << setw(9) << setprecision(6) << (cross4332);
    cout << "\n\n";
#endif
    // First elements of coordinate triples
    tempCoord5 << -((cross1223 * scaleFactor) / (norm12 * sinAngle123 * sinAngle123));
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index1)*3)+j) = tempCoord5(j);
    }
    // Second elements of coordinate triples
    tempCoord5 << ((norm23 - (norm12 * cosAngle123)) / (norm23 * norm12 * sinAngle123 * sinAngle123)) * (cross1223);
    tempCoord6 << (cosAngle234 / (norm23 * sinAngle234 * sinAngle234)) * (cross4332);
#ifdef DEBUG
    cout << "tempCoord5:\n";
    cout << setw(9) << setprecision(6) << (tempCoord5);
    cout << "tempCoord6:\n";
    cout << setw(9) << setprecision(6) << (tempCoord6);
#endif
    tempCoord5 << (tempCoord5 + tempCoord6) * scaleFactor;
#ifdef DEBUG
    cout << "tempCoord5:\n";
    cout << setw(9) << setprecision(6) << (tempCoord5);
#endif
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index2)*3)+j) = tempCoord5(j);
    }
    // Third elements of coordinate triples
    tempCoord5 << ((norm32 - (norm43 * cosAngle234)) / (norm32 * norm43 * sinAngle234 * sinAngle234)) * (cross4332);
    tempCoord6 << (cosAngle123 / (norm32 * sinAngle123 * sinAngle123)) * (cross1223);
    tempCoord5 << (tempCoord5 + tempCoord6) * scaleFactor;
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index3)*3)+j) = tempCoord5(j);
    }
    // Fourth elements of coordinate triples
    tempCoord5 << -((cross4332 * scaleFactor) / (norm43 * sinAngle234 * sinAngle234));
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+1,((index4)*3)+j) = tempCoord5(j);
    }
  }
  
#ifdef DEBUG
  cout << "B:\n";
  cout << setw(9) << setprecision(3) << (B);
  cout << "\n\n";
#endif

  // Impropers
  RowVector r41(3);   // Vector from 4th to 1st point
  RowVector r42(3);   // Vector from 4th to 2nd point
  RowVector e41(3);   // Unit vector from 4th to 1st point
  RowVector e42(3);   // Unit vector from 4th to 2nd point
  RowVector normVector(3);   // Normal to the plane
  Real norm41;        // Norm of r41
  Real norm42;        // Norm of r42
  Real angle142 = 0.0;   // Angle in radians
  Real angle143 = 0.0;   // Angle in radians
  Real angle243 = 0.0;   // Angle in radians
  Real cosAngle142 = 0.0;
  Real cosAngle143 = 0.0;
  Real cosAngle243 = 0.0;
  Real sinAngle142 = 0.0;
  Real sinAngle143 = 0.0;
  Real sinAngle243 = 0.0;
  Real apexCoeff = 0.0;   // Magnitude of central atom displacement
  scaleFactor = -0.352313;   // Scale factor (-0.352313)
  for (i=0; i<numImpropers; i++) {
    index1 = impropers[i]->x1;
    index2 = impropers[i]->x2;
    index3 = impropers[i]->x3;
    index4 = impropers[i]->x4;
    for (j=0; j<3; j++) {
      tempCoord1(j+1) = cartCoords[index1][j];
      tempCoord2(j+1) = cartCoords[index2][j];
      tempCoord3(j+1) = cartCoords[index3][j];
      tempCoord4(j+1) = cartCoords[index4][j];
    }
    r41 << tempCoord1 - tempCoord4;
    r42 << tempCoord2 - tempCoord4;
    r43 << tempCoord3 - tempCoord4;
    norm41 = r41.NormFrobenius();
    norm42 = r42.NormFrobenius();
    norm43 = r43.NormFrobenius();
    e41 << r41 / norm41;
    e42 << r42 / norm42;
    e43 << r43 / norm43;
    angle142 = acos(DotProduct(r41,r42) / (norm41 * norm42));
    angle143 = acos(DotProduct(r41,r43) / (norm41 * norm43));
    angle243 = acos(DotProduct(r42,r43) / (norm42 * norm43));
    cosAngle142 = DotProduct(r41,r42) / (norm41 * norm42);
    cosAngle143 = DotProduct(r41,r43) / (norm41 * norm43);
    cosAngle243 = DotProduct(r42,r43) / (norm42 * norm43);
    sinAngle142 = sqrt(1 - (cosAngle142 * cosAngle142));
    sinAngle143 = sqrt(1 - (cosAngle143 * cosAngle143));
    sinAngle243 = sqrt(1 - (cosAngle243 * cosAngle243));
    normVector(1) = (r41(2)*r42(3)) - (r41(3)*r42(2));
    normVector(2) = (r41(3)*r42(1)) - (r41(1)*r42(3));
    normVector(3) = (r41(1)*r42(2)) - (r41(2)*r42(1));
    normVector << normVector / normVector.NormFrobenius();
    // First elements of coordinate triples
    tempCoord5 << normVector * (scaleFactor / norm41);
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index1)*3)+j) = tempCoord5(j);
    }
    // Second elements of coordinate triples
    tempCoord5 << normVector * sinAngle143 * scaleFactor;
    tempCoord5 << tempCoord5 / (norm42 * sinAngle243);
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index2)*3)+j) = tempCoord5(j);
    }
    // Third elements of coordinate triples
    tempCoord5 << normVector * sinAngle142 * scaleFactor;
    tempCoord5 << tempCoord5 / (norm43 * sinAngle243);
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index3)*3)+j) = tempCoord5(j);
    }
    // Fourth elements of coordinate triples
    apexCoeff = -1.0 / norm42;
    apexCoeff -= sinAngle143 / (norm42 * sinAngle243);
    apexCoeff -= sinAngle142 / (norm43 * sinAngle243);
    tempCoord5 << normVector * apexCoeff * scaleFactor;
    for (j=1; j<=3; j++) {
      B(i+numBonds+numAngles+numDihedrals+1,((index4)*3)+j) = tempCoord5(j);
    }
  }
  
  return;
}