void calculateCenterOfMass(cellptr cell)
{
/* Center of mass position */
vector cmPos;
vector tempV;
nodeptr q;
int i;
/* Init the cell's total mass */
Mass(cell) = 0.0;
/* Init the cell's center of mass position */
CLRV(cmPos);
/* Loop over the subnodes */
for (i = 0; i < NCHILD; i++) {
/* Skipping empty child nodes */
if ((q = Child(cell)[i]) != NULL) {
/* If node is a cell */
if (Type(q) == CELL)
/* Recursively do the same */
calculateCenterOfMass((cellptr) q);
/* Accumulate total mass */
Mass(cell) = Mass(q);
/* Accumulate center of mass */
ADDMULVS(cmPos, Pos(q), Mass(q));
}
}
/* Usually, cell has mass */
if (Mass(cell) > 0.0) {
/* Find center of mass position */
DIVVS(cmPos, cmPos, Mass(cell));
} else { /* If no mass inside */
SETV(cmPos, Pos(cell));
}
SETV(Pos(cell), cmPos);
}
PolygonBody::PolygonBody(const std::vector<Vector2f>& vertices, const Vector2f& position, float mass, float momentOfInertia, float restitution, float staticFriction, float dynamicFriction) :
RigidBody(position, 0.0f, mass, momentOfInertia, restitution, staticFriction, dynamicFriction) {
Vector2f absolutePosition = calculateCenterOfMass(vertices);
for (std::vector<Vector2f>::const_iterator i = vertices.begin(); i != vertices.end(); ++i)
_verticesModelSpace.push_back((*i) - absolutePosition);
_verticesWorldSpace.resize(_verticesModelSpace.size());
updateVerticesWorldSpace();
_radius = calculateRadiusFromCenterOfMass();
}
void treeInit(cellptr * root, bodyptr bodies, int nbody, real * rootSize)
{
bodyptr p;
(*rootSize) = 1.0;
/* Count the root size */
(*rootSize) = expandBox(*root, *rootSize, bodies, nbody);
(*root) = treeCreateCell();
/* Foreach bodies */
int i = 0;
for (p = bodies; p < bodies + nbody; p++) {
/* Insert into tree */
treeInsert(root, *rootSize, p);
}
calculateCenterOfMass(*root);
makeThread((nodeptr) * root, NULL);
}
Triangle::Triangle(GLfloat vertices[], Color& color) :
_color(color)
{
for (int i = 0; i < _verticesLenght; i++)
{
_vertecies[i] = vertices[i];
}
_centreOfMass = calculateCenterOfMass();
ModelMatrix = glm::mat4(1.0f);
glGenVertexArrays(1, &_vertexObjectArray);
glBindVertexArray(_vertexObjectArray);
glGenBuffers(1, &_vertexBuffer);
glBindBuffer(GL_ARRAY_BUFFER, _vertexBuffer);
glBufferData(GL_ARRAY_BUFFER, sizeof(_vertecies), _vertecies, GL_STATIC_DRAW);
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, _vertexBuffer);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, (void*)0);
glDisableVertexAttribArray(0);
}
void rotateCoordinates(ligand *lig, ligand *ligNew, box *pbc, float stepSize, float *minR, float *maxR)
{
/*
printf("CA_CA -- in rotateCoordinates: minR: %f %f %f maxR: %f %f %f \n", minR[0], minR[1], minR[2], maxR[0], maxR[1], maxR[2]);
int t;
for (t=0; t<lig->nAtoms; t++)
{
printf("CA_CA at start of rotateCoordinates i %d lig->atom[i].r %f %f %f\n", t, lig->atom[t].r[0], lig->atom[t].r[1], lig->atom[t].r[2]);
}
printf("CA_CA \n");
*/
float phi, psi, theta;
float Tx[9], Ty[9], Tz[9], Txy[9], T[9];
float Rcom[3];
int i, j;
theta = stepSize*((float)rand()/RAND_MAX);
phi = stepSize*((float)rand()/RAND_MAX);
psi = stepSize*((float)rand()/RAND_MAX);
calculateTransformationMatrixRotateXaxis(Tx, theta);
calculateTransformationMatrixRotateYaxis(Ty, phi);
calculateTransformationMatrixRotateZaxis(Tz, psi);
multiplyMatrices(Ty, Tx, Txy, 3, 3, 3);
multiplyMatrices(Tz, Txy, T, 3, 3, 3);
//for (i=0; i<3; i++)
//printf("%f %f %f\n", T[3*i], T[3*i+1], T[3*i+2]);
// makeMoleculeWhole(lig, ligNew, pbc);
/*
for (t=0; t<lig->nAtoms; t++)
{
printf("CA_CA after makeMoleculeWhole %d ligNew->atom[i].r %f %f %f\n", t, ligNew->atom[t].r[0], ligNew->atom[t].r[1], ligNew->atom[t].r[2]);
}
printf("CA_CA \n");
*/
/*
// If molecule is NOT actually whole (ie, if unrealistic bond
// lengths), then there's a problem, so exit out of program
int lig_res = 330;
int lig_atom;
int lig_atom_to_print = 0;
double CA_CA_bond_dist_in_lig;
double del_x_sqrd;
double del_y_sqrd;
double del_z_sqrd;
for (lig_atom=0; lig_atom < ligNew->nAtoms; lig_atom++)
{
// Print a record of all CA-CA bond distances in the final ligand configuration:
if (lig_atom>0)
{
del_x_sqrd = (ligNew->atom[lig_atom].r[0]-ligNew->atom[lig_atom-1].r[0]) * (ligNew->atom[lig_atom].r[0]-ligNew->atom[lig_atom-1].r[0]);
del_y_sqrd = (ligNew->atom[lig_atom].r[1]-ligNew->atom[lig_atom-1].r[1]) * (ligNew->atom[lig_atom].r[1]-ligNew->atom[lig_atom-1].r[1]);
del_z_sqrd = (ligNew->atom[lig_atom].r[2]-ligNew->atom[lig_atom-1].r[2]) * (ligNew->atom[lig_atom].r[2]-ligNew->atom[lig_atom-1].r[2]);
CA_CA_bond_dist_in_lig = sqrt(del_x_sqrd + del_y_sqrd + del_z_sqrd);
printf("CA_CA_bond_dist_in_lig: %f \n", CA_CA_bond_dist_in_lig); //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//if (CA_CA_bond_dist_in_lig < 3.6 || CA_CA_bond_dist_in_lig > 4.0)
if (CA_CA_bond_dist_in_lig > 4.0)
{
printf("CA_CA extreme bond dist\n");
exit("\nEXIT\n");
}
}
lig_res += 1;
}
printf("CA_CA \n\n\n", t, lig->atom[t].r[0], lig->atom[t].r[1], lig->atom[t].r[2]);
*/
calculateCenterOfMass(ligNew, Rcom);
//printf("ligNew\n");
//for (i=0; i<lig->nAtoms; i++) {
//for (j=0; j<3; j++) {
//printf("%f ", ligNew->atom[i].r[j]);
//}
//printf("\n");
//}
//printf("Rcom - %f %f %f\n", Rcom[0], Rcom[1], Rcom[2]);
for (i=0; i<lig->nAtoms; i++)
{
for (j=0; j<3; j++)
{
ligNew->atom[i].r[j] = ligNew->atom[i].r[j] - Rcom[j];
}
//printf("i %d ligNew->atom[i].r %f %f %f\n", i, ligNew->atom[i].r[0], ligNew->atom[i].r[1], ligNew->atom[i].r[2]);
}
/*
for (t=0; t<lig->nAtoms; t++)
{
printf("CA_CA after subtracting Rcoms %d ligNew->atom[i].r %f %f %f\n", t, ligNew->atom[t].r[0], ligNew->atom[t].r[1], ligNew->atom[t].r[2]);
}
printf("CA_CA \n");
*/
rotateCoordinatesTransformationMatrix(ligNew, T);
/*
for (t=0; t<lig->nAtoms; t++)
{
printf("CA_CA after rotateCoordinatesTransformationMatrix %d ligNew->atom[i].r %f %f %f\n", t, ligNew->atom[t].r[0], ligNew->atom[t].r[1], ligNew->atom[t].r[2]);
}
printf("CA_CA \n");
*/
for (i=0; i<lig->nAtoms; i++)
{
for (j=0; j<3; j++)
{
ligNew->atom[i].r[j] = ligNew->atom[i].r[j] + Rcom[j];
}
}
/*
for (t=0; t<lig->nAtoms; t++)
{
printf("CA_CA after adding Rcoms %d ligNew->atom[i].r %f %f %f\n", t, ligNew->atom[t].r[0], ligNew->atom[t].r[1], ligNew->atom[t].r[2]);
}
printf("CA_CA \n");
*/
transformCoordinatesToSameBox(ligNew, minR, maxR, pbc);
/*
for (t=0; t<lig->nAtoms; t++)
{
printf("CA_CA after transformCoordinatesToSameBox %d ligNew->atom[i].r %f %f %f\n", t, ligNew->atom[t].r[0], ligNew->atom[t].r[1], ligNew->atom[t].r[2]);
}
*/
return;
}
//-=========================================================
// Arguments: in_pVectors - source vectors to be
// quantized
// in_numInputVectors - number of input Vectors
// out_pOutputVectors - array for returned vectors
// io_pNumOutputVectors - On input, contains
// the quantization target,
// i.e., the number of desired
// "best" vectors. On output,
// contains the number that
// were actually created.
//
// Returns: VQ_SUCCESS or VQ_FAILURE
//
// Description: Uses PCA to quantize the inputs to the
// "best" set of representative vectors.
//
// Notes: Note that the CALLING function is responsible
// for allocating the memory assigned to the
// elements of the output vector array. This
// function neither allocates nor free's any
// memory for that purpose.
//-=========================================================
int
quantizeVectors(const quantVector* in_pVectors,
const Int32 in_numInputVectors,
quantVector* out_pOutputVectors,
Int32& io_rNumOutputVectors)
{
// Sanity checks
//
AssertFatal(in_pVectors != NULL, "NULL vector input");
AssertFatal(in_numInputVectors > 0, "Bad numVectors");
AssertFatal(out_pOutputVectors != NULL, "output Array NULL");
AssertFatal(io_rNumOutputVectors > 0, "Error, bad quant request");
if (in_numInputVectors <= io_rNumOutputVectors) {
// Well, in this case we have an easy time of it!
// Just copy the input vectors to the output
// array and be done with it! (return rather.)
//
for (int i = 0; i < in_numInputVectors; i++) {
out_pOutputVectors[i].numDim = in_pVectors[i].numDim;
memcpy(out_pOutputVectors[i].pElem, in_pVectors[i].pElem,
sizeof(double) * in_pVectors[i].numDim);
}
io_rNumOutputVectors = in_numInputVectors;
return VQ_SUCCESS;
}
// Ah well, on to the real work. First, create the
// quantization table, with the appropriate number
// of entries, and insert the entire input set into
// the first entry.
//
quantTableEntry *pQuantEntries = new quantTableEntry[io_rNumOutputVectors];
int currEndOfTable = 0;
// Create the sortVector array that we will use to sort the
// input quantVectors
//
sortVector *pSortVectors = new sortVector[in_numInputVectors];
for (int v = 0; v < in_numInputVectors; v++) {
pSortVectors[v].pQVector = &in_pVectors[v];
}
// Create the base quantEntry
//
createQuantTableEntry(&pQuantEntries[0], pSortVectors,
in_numInputVectors);
// Ok, now the meat of the process. While there are fewer
// than the desired number of output entries, find the
// entry that maximizes (STD_DEV * numVectors), and
// split it into two sub-partitions.
// Note that we are guaranteed to always have a partition with
// > 1 entries at all times during this loop, so there is never
// a chance of trying to split a singular partition. However,
// the danger exists that we could wind up with partitions that
// contain multilple copies of the same vector, so we check for
// zero STD_DEV, and abort at that point...
//
while (++currEndOfTable < io_rNumOutputVectors) {
int maxEntry = -1;
double maxStdDev = -1.0;
// Find "worst" table entry
//
for (int i = 0; i < currEndOfTable; i++) {
double temp;
if (pQuantEntries[i].standardDev < 0) {
temp = getStandardDeviation(&pQuantEntries[i]);
temp *= pQuantEntries[i].totalWeight;
pQuantEntries[i].standardDev = temp;
} else {
temp = pQuantEntries[i].standardDev;
}
if (temp > maxStdDev) {
maxStdDev = temp;
maxEntry = i;
}
}
// Check to make sure that we haven't homogenized our entries
//
const double minStdDev = 1e-10;
if (maxStdDev <= minStdDev) {
// Ok, break out of the while loop, we're done.
//
break;
}
sortQuantEntry(&pQuantEntries[maxEntry]);
splitQuantEntry(&pQuantEntries[maxEntry],
&pQuantEntries[currEndOfTable]);
}
// Ok. At this point, we have a number of partitioned vectors,
// in nice neat buckets, (currEndOfTable) in number. Write
// the number of output vectors in io_rNumOutputVectors,
// extract the center of masses from the buckets, and put these
// into the output array.
//
io_rNumOutputVectors = currEndOfTable;
for (int i = 0; i < io_rNumOutputVectors; i++) {
calculateCenterOfMass(&pQuantEntries[i], &out_pOutputVectors[i]);
}
// Yay! Done and Successful! Clean-up section...
//
delete [] pSortVectors;
delete [] pQuantEntries;
return VQ_SUCCESS;
}
