static GLboolean r300RunRender(GLcontext * ctx,
struct tnl_pipeline_stage *stage)
{
r300ContextPtr rmesa = R300_CONTEXT(ctx);
struct radeon_vertex_buffer *VB = &rmesa->state.VB;
int i;
int cmd_reserved = 0;
int cmd_written = 0;
drm_radeon_cmd_header_t *cmd = NULL;
if (RADEON_DEBUG & DEBUG_PRIMS)
fprintf(stderr, "%s\n", __FUNCTION__);
if (stage) {
TNLcontext *tnl = TNL_CONTEXT(ctx);
radeon_vb_to_rvb(rmesa, VB, &tnl->vb);
}
r300UpdateShaders(rmesa);
if (r300EmitArrays(ctx))
return GL_TRUE;
r300UpdateShaderStates(rmesa);
reg_start(R300_RB3D_DSTCACHE_CTLSTAT, 0);
e32(R300_RB3D_DSTCACHE_UNKNOWN_0A);
reg_start(R300_RB3D_ZCACHE_CTLSTAT, 0);
e32(R300_RB3D_ZCACHE_UNKNOWN_03);
r300EmitState(rmesa);
for (i = 0; i < VB->PrimitiveCount; i++) {
GLuint prim = _tnl_translate_prim(&VB->Primitive[i]);
GLuint start = VB->Primitive[i].start;
GLuint end = VB->Primitive[i].start + VB->Primitive[i].count;
r300RunRenderPrimitive(rmesa, ctx, start, end, prim);
}
reg_start(R300_RB3D_DSTCACHE_CTLSTAT, 0);
e32(R300_RB3D_DSTCACHE_UNKNOWN_0A);
reg_start(R300_RB3D_ZCACHE_CTLSTAT, 0);
e32(R300_RB3D_ZCACHE_UNKNOWN_03);
#ifdef USER_BUFFERS
r300UseArrays(ctx);
#endif
r300ReleaseArrays(ctx);
return GL_FALSE;
}
static void r300FireEB(r300ContextPtr rmesa, unsigned long addr,
int vertex_count, int type)
{
int cmd_reserved = 0;
int cmd_written = 0;
drm_radeon_cmd_header_t *cmd = NULL;
start_packet3(CP_PACKET3(R300_PACKET3_3D_DRAW_INDX_2, 0), 0);
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES | (vertex_count << 16) | type | R300_VAP_VF_CNTL__INDEX_SIZE_32bit);
start_packet3(CP_PACKET3(R300_PACKET3_INDX_BUFFER, 2), 2);
e32(R300_EB_UNK1 | (0 << 16) | R300_EB_UNK2);
e32(addr);
e32(vertex_count);
}
static void r300FireAOS(r300ContextPtr rmesa, int vertex_count, int type)
{
int cmd_reserved = 0;
int cmd_written = 0;
drm_radeon_cmd_header_t *cmd = NULL;
start_packet3(CP_PACKET3(R300_PACKET3_3D_DRAW_VBUF_2, 0), 0);
e32(R300_VAP_VF_CNTL__PRIM_WALK_VERTEX_LIST | (vertex_count << 16) | type);
}
static void r300EmitAOS(r300ContextPtr rmesa, GLuint nr, GLuint offset)
{
int sz = 1 + (nr >> 1) * 3 + (nr & 1) * 2;
int i;
int cmd_reserved = 0;
int cmd_written = 0;
drm_radeon_cmd_header_t *cmd = NULL;
if (RADEON_DEBUG & DEBUG_VERTS)
fprintf(stderr, "%s: nr=%d, ofs=0x%08x\n", __FUNCTION__, nr,
offset);
start_packet3(CP_PACKET3(R300_PACKET3_3D_LOAD_VBPNTR, sz - 1), sz - 1);
e32(nr);
for (i = 0; i + 1 < nr; i += 2) {
e32((rmesa->state.aos[i].aos_size << 0) |
(rmesa->state.aos[i].aos_stride << 8) |
(rmesa->state.aos[i + 1].aos_size << 16) |
(rmesa->state.aos[i + 1].aos_stride << 24));
e32(rmesa->state.aos[i].aos_offset + offset * 4 * rmesa->state.aos[i].aos_stride);
e32(rmesa->state.aos[i + 1].aos_offset + offset * 4 * rmesa->state.aos[i + 1].aos_stride);
}
if (nr & 1) {
e32((rmesa->state.aos[nr - 1].aos_size << 0) |
(rmesa->state.aos[nr - 1].aos_stride << 8));
e32(rmesa->state.aos[nr - 1].aos_offset + offset * 4 * rmesa->state.aos[nr - 1].aos_stride);
}
}
static void r300_render_vb_primitive(r300ContextPtr rmesa,
GLcontext *ctx,
int start,
int end,
int prim)
{
int type, num_verts;
type=r300_get_primitive_type(rmesa, ctx, prim);
num_verts=r300_get_num_verts(rmesa, ctx, end-start, prim);
if(type<0 || num_verts <= 0)return;
if(rmesa->state.VB.Elts){
r300EmitAOS(rmesa, rmesa->state.aos_count, /*0*/start);
#if 0
LOCAL_VARS
int i;
start_index32_packet(num_verts, type);
for(i=0; i < num_verts; i++)
e32(((unsigned long *)rmesa->state.VB.Elts)[i]/*rmesa->state.Elts[start+i]*/); /* start ? */
#else
if(num_verts == 1){
//start_index32_packet(num_verts, type);
//e32(rmesa->state.Elts[start]);
return;
}
if(num_verts > 65535){ /* not implemented yet */
WARN_ONCE("Too many elts\n");
return;
}
r300EmitElts(ctx, rmesa->state.VB.Elts, num_verts, rmesa->state.VB.elt_size);
fire_EB(PASS_PREFIX rmesa->state.elt_dma.aos_offset, num_verts, type, rmesa->state.VB.elt_size);
#endif
}else{
r300EmitAOS(rmesa, rmesa->state.aos_count, start);
fire_AOS(PASS_PREFIX num_verts, type);
}
}
GLboolean r300_run_vb_render(GLcontext *ctx,
struct tnl_pipeline_stage *stage)
{
r300ContextPtr rmesa = R300_CONTEXT(ctx);
struct radeon_vertex_buffer *VB = &rmesa->state.VB;
int i;
LOCAL_VARS
if (RADEON_DEBUG & DEBUG_PRIMS)
fprintf(stderr, "%s\n", __FUNCTION__);
if (stage) {
TNLcontext *tnl = TNL_CONTEXT(ctx);
radeon_vb_to_rvb(rmesa, VB, &tnl->vb);
}
r300UpdateShaders(rmesa);
if (rmesa->state.VB.LockCount == 0 || 1) {
r300ReleaseArrays(ctx);
r300EmitArrays(ctx, GL_FALSE);
r300UpdateShaderStates(rmesa);
} else {
/* TODO: Figure out why do we need these. */
R300_STATECHANGE(rmesa, vir[0]);
R300_STATECHANGE(rmesa, vir[1]);
R300_STATECHANGE(rmesa, vic);
R300_STATECHANGE(rmesa, vof);
#if 0
fprintf(stderr, "dt:\n");
for(i=0; i < VERT_ATTRIB_MAX; i++){
fprintf(stderr, "dt %d:", i);
dump_dt(&rmesa->state.VB.AttribPtr[i], VB->Count);
}
fprintf(stderr, "before:\n");
for(i=0; i < rmesa->state.aos_count; i++){
fprintf(stderr, "aos %d:", i);
dump_array(&rmesa->state.aos[i], VB->Count);
}
#endif
#if 0
r300ReleaseArrays(ctx);
r300EmitArrays(ctx, GL_FALSE);
fprintf(stderr, "after:\n");
for(i=0; i < rmesa->state.aos_count; i++){
fprintf(stderr, "aos %d:", i);
dump_array(&rmesa->state.aos[i], VB->Count);
}
#endif
}
reg_start(R300_RB3D_DSTCACHE_CTLSTAT,0);
e32(0x0000000a);
reg_start(0x4f18,0);
e32(0x00000003);
#if 0
reg_start(R300_VAP_PVS_WAITIDLE,0);
e32(0x00000000);
#endif
r300EmitState(rmesa);
for(i=0; i < VB->PrimitiveCount; i++){
GLuint prim = VB->Primitive[i].mode;
GLuint start = VB->Primitive[i].start;
GLuint length = VB->Primitive[i].count;
r300_render_vb_primitive(rmesa, ctx, start, start + length, prim);
}
reg_start(R300_RB3D_DSTCACHE_CTLSTAT,0);
e32(0x0000000a/*0x2*/);
reg_start(0x4f18,0);
e32(0x00000003/*0x1*/);
#ifdef USER_BUFFERS
r300UseArrays(ctx);
#endif
return GL_FALSE;
}
static void inline fire_EB(PREFIX unsigned long addr, int vertex_count, int type, int elt_size)
{
LOCAL_VARS
unsigned long addr_a;
unsigned long t_addr;
unsigned long magic_1, magic_2;
GLcontext *ctx;
ctx = rmesa->radeon.glCtx;
assert(elt_size == 2 || elt_size == 4);
if(addr & (elt_size-1)){
WARN_ONCE("Badly aligned buffer\n");
return ;
}
#ifdef OPTIMIZE_ELTS
addr_a = 0;
magic_1 = (addr % 32) / 4;
t_addr = addr & (~0x1d);
magic_2 = (vertex_count + 1 + (t_addr & 0x2)) / 2 + magic_1;
check_space(6);
start_packet3(RADEON_CP_PACKET3_3D_DRAW_INDX_2, 0);
if(elt_size == 4){
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES | (vertex_count<<16) | type | R300_VAP_VF_CNTL__INDEX_SIZE_32bit);
} else {
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES | (vertex_count<<16) | type);
}
start_packet3(RADEON_CP_PACKET3_INDX_BUFFER, 2);
if(elt_size == 4){
e32(R300_EB_UNK1 | (0 << 16) | R300_EB_UNK2);
e32(addr /*& 0xffffffe3*/);
} else {
e32(R300_EB_UNK1 | (magic_1 << 16) | R300_EB_UNK2);
e32(t_addr);
}
if(elt_size == 4){
e32(vertex_count /*+ addr_a/4*/); /* Total number of dwords needed? */
} else {
e32(magic_2); /* Total number of dwords needed? */
}
//cp_delay(PASS_PREFIX 1);
#if 0
fprintf(stderr, "magic_1 %d\n", magic_1);
fprintf(stderr, "t_addr %x\n", t_addr);
fprintf(stderr, "magic_2 %d\n", magic_2);
exit(1);
#endif
#else
(void)magic_2, (void)magic_1, (void)t_addr;
addr_a = 0;
check_space(6);
start_packet3(RADEON_CP_PACKET3_3D_DRAW_INDX_2, 0);
if(elt_size == 4){
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES | (vertex_count<<16) | type | R300_VAP_VF_CNTL__INDEX_SIZE_32bit);
} else {
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES | (vertex_count<<16) | type);
}
start_packet3(RADEON_CP_PACKET3_INDX_BUFFER, 2);
e32(R300_EB_UNK1 | (0 << 16) | R300_EB_UNK2);
e32(addr /*& 0xffffffe3*/);
if(elt_size == 4){
e32(vertex_count /*+ addr_a/4*/); /* Total number of dwords needed? */
} else {
e32((vertex_count+1)/2 /*+ addr_a/4*/); /* Total number of dwords needed? */
}
//cp_delay(PASS_PREFIX 1);
#endif
}
static void r300FireEB(r300ContextPtr rmesa, unsigned long addr,
int vertex_count, int type, int elt_size)
{
int cmd_reserved = 0;
int cmd_written = 0;
drm_radeon_cmd_header_t *cmd = NULL;
unsigned long t_addr;
unsigned long magic_1, magic_2;
assert(elt_size == 2 || elt_size == 4);
if (addr & (elt_size - 1)) {
WARN_ONCE("Badly aligned buffer\n");
return;
}
magic_1 = (addr % 32) / 4;
t_addr = addr & ~0x1d;
magic_2 = (vertex_count + 1 + (t_addr & 0x2)) / 2 + magic_1;
start_packet3(RADEON_CP_PACKET3_3D_DRAW_INDX_2, 0);
if (elt_size == 4) {
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES |
(vertex_count << 16) | type |
R300_VAP_VF_CNTL__INDEX_SIZE_32bit);
} else {
e32(R300_VAP_VF_CNTL__PRIM_WALK_INDICES |
(vertex_count << 16) | type);
}
start_packet3(RADEON_CP_PACKET3_INDX_BUFFER, 2);
#ifdef OPTIMIZE_ELTS
if (elt_size == 4) {
e32(R300_EB_UNK1 | (0 << 16) | R300_EB_UNK2);
e32(addr);
} else {
e32(R300_EB_UNK1 | (magic_1 << 16) | R300_EB_UNK2);
e32(t_addr);
}
#else
e32(R300_EB_UNK1 | (0 << 16) | R300_EB_UNK2);
e32(addr);
#endif
if (elt_size == 4) {
e32(vertex_count);
} else {
#ifdef OPTIMIZE_ELTS
e32(magic_2);
#else
e32((vertex_count + 1) / 2);
#endif
}
}
/**
* 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;
}
