/**
* Draw a stick between two points.
*
* This function draws a cylincer and a sphere on its top
* between two points.
*
* @param point1 is the first point.
* @param point2 is the second point where is going to be the
* sphere.
*/
void NeutralModel :: drawLimp (XnPoint3D& point1, XnPoint3D& point2)
{
Vector3D v;
Vector3D u;
Vector3D w;
float limpLength;
GLUquadricObj *quadratic;
quadratic = gluNewQuadric();
gluQuadricNormals(quadratic, GLU_SMOOTH);
gluQuadricOrientation(quadratic,GLU_OUTSIDE);
v = Vector3D (point1);
u = Vector3D (point2);
w = u - v;
limpLength = w.magnitude();
glPushMatrix();
orientMatrix(point1, point2);
gluCylinder(quadratic, 15.0f, 15.0f,limpLength, 10, 1);
glPopMatrix();
glPushMatrix();
glTranslatef(point2.X, point2.Y, point2.Z);
glutSolidSphere(20.0, 10, 10);
glPopMatrix();
}
TEST(Vector3D, Normalization)
{
Vector3D vec(1.0f, 2.0f, 3.0f);
Vector3D normalized = vec.normalized();
EXPECT_FLOAT_EQ(normalized.magnitude(), 1.0f);
EXPECT_FLOAT_EQ(normalized.x, 0.26726124191242438468455348087975f);
EXPECT_FLOAT_EQ(normalized.y, 0.53452248382484876936910696175951f);
EXPECT_FLOAT_EQ(normalized.z, 0.80178372573727315405366044263926f);
vec = Vector3D(-2.8f, 8.4f, -3.14f);
normalized = vec.normalized();
EXPECT_FLOAT_EQ(normalized.magnitude(), 1.0f);
EXPECT_FLOAT_EQ(normalized.x, -0.2980417151042440347087515078823f);
EXPECT_FLOAT_EQ(normalized.y, 0.89412514531273210412625452364691f);
EXPECT_FLOAT_EQ(normalized.z, -0.3342324947954736674948141909823f);
}
bool Renderer::isChunkVisible(Vector3D origin, Vector3D pos)
{
Vector3D dist = pos - origin;
if(dist.magnitude() <= 4.0)
return true;
return false;
}
void ParticleDrag::updateForce(Particle* particle, double dElapsedFrameTime)
{
Vector3D force;
particle->getVelocity(&force);
// Calculate the total drag coefficient
double dragCoeff = force.magnitude();
dragCoeff = k1 * dragCoeff + k2 * dragCoeff * dragCoeff;
// Calculate the final force and apply it
force.normalize();
force *= -dragCoeff;
particle->addForce(force);
}
void ParticleAnchoredSpring::updateForce(Particle* particle, double dElapsedFrameTime)
{
// Calculate the vector of the spring
Vector3D force;
particle->getPosition(&force);
force -= *anchor;
// Calculate the magnitude of the force
double magnitude = force.magnitude();
magnitude = (restLength - magnitude) * springConstant;
// Calculate the final force and apply it
force.normalize();
force *= magnitude;
particle->addForce(force);
}
void ParticleSpring::updateForce(Particle* particle, double dElapsedFrameTime)
{
// Calculate the vector of the spring
Vector3D force;
particle->getPosition(&force);
force -= other->getPosition();
// Calculate the magnitude of the force
double magnitude = force.magnitude();
magnitude = abs(magnitude - restLength);
magnitude *= springConstant;
// Calculate the final force and apply it
force.normalize();
force *= -magnitude;
particle->addForce(force);
}
void ParticleBungee::updateForce(Particle* particle, double dElapsedFrameTime)
{
// Calculate the vector of the spring
Vector3D force;
particle->getPosition(&force);
force -= other->getPosition();
// Check if the bungee is compressed
double magnitude = force.magnitude();
if (magnitude <= restLength) return;
// Calculate the magnitude of the force
magnitude = springConstant * (restLength - magnitude);
// Calculate the final force and apply it
force.normalize();
force *= -magnitude;
particle->addForce(force);
}
bool Interstitial::minimizePoint(Vector3D& point, const ISO& iso, double scale)
{
// Loop until converged
int i;
int minIndex;
int loopNum = 0;
int maxLoops = 50;
int curNegCount;
int origNegCount;
double mag;
double tol = 1e-8;
double stepSize = 1;
double minEigenvalue;
Vector3D step;
Vector3D deriv;
Vector3D eigenvalues;
Vector3D minEigenvector;
Matrix3D eigenvectors;
Matrix3D hessian;
for (; loopNum < maxLoops; ++loopNum)
{
// Get current step, derivatives, and second derivatives
step = getStep(iso, point, deriv, hessian, scale);
// Check if at a stationary point
if (deriv.magnitude() < tol)
{
// Count the number of negative eigenvalues
origNegCount = 0;
eigenvalues = hessian.eigenvalues(&eigenvectors);
for (i = 0; i < 3; ++i)
{
if (eigenvalues[i] < -tol)
++origNegCount;
}
// Found a minimum so break
if (origNegCount == 0)
break;
// Get the most negative eigenvalue
minIndex = 0;
if (eigenvalues[1] < eigenvalues[minIndex])
minIndex = 1;
if (eigenvalues[2] < eigenvalues[minIndex])
minIndex = 2;
minEigenvalue = eigenvalues[minIndex];
minEigenvector[0] = eigenvectors(0, minIndex);
minEigenvector[1] = eigenvectors(1, minIndex);
minEigenvector[2] = eigenvectors(2, minIndex);
// Make step along lowest eigenvector
step = minEigenvector * stepSize;
if (step.magnitude() < tol)
{
loopNum = maxLoops;
break;
}
point -= step;
// Perform gradient based minimization until there is one less negative eigenvalue
for (++loopNum; loopNum < maxLoops; ++loopNum)
{
// Get current derivative
getStep(iso, point, deriv, hessian, scale);
// Count the number of negative eigenvalues
curNegCount = 0;
eigenvalues = hessian.eigenvalues();
for (i = 0; i < 3; ++i)
{
if (eigenvalues[i] < -tol)
++curNegCount;
}
// Break if a negative eigenvalue was removed
if (curNegCount < origNegCount)
break;
// Make step
step = deriv;
step *= stepSize / deriv.magnitude();
point -= step;
}
}
// Make sure that step is not too large and update position
else
{
mag = step.magnitude();
if (mag > 0.25)
step *= 0.25 / mag;
point -= step;
}
}
// Return that point was not found if not converged
if (loopNum >= maxLoops)
return false;
// Return that point was found
return true;
}
void CameraMovement::moveCamera(Camera& camera, Vector3D deltaV) {
static Vector3D camVelocity(0,0,40);
camVelocity = camVelocity.plus(deltaV);
if (camVelocity.magnitude() > 0) {
Vector3D newCamLocation = camera.getLocation().plus(camVelocity);
// check known bounds
if (newCamLocation.z() < m_minimumCameraZ) { newCamLocation.z() = m_minimumCameraZ; }
if (newCamLocation.x() < m_boundsMin.x()) { newCamLocation.x() = m_boundsMin.x(); }
else if (newCamLocation.x() > m_boundsMax.x()) { newCamLocation.x() = m_boundsMax.x(); }
if (newCamLocation.y() < m_boundsMin.y()) { newCamLocation.y() = m_boundsMin.y(); }
else if (newCamLocation.y() > m_boundsMax.y()) { newCamLocation.y() = m_boundsMax.y(); }
// Check pyramid bounds
// calculate the x,y portion of pyramid center/top
Vector3D pyramidTop = m_boundsMax.plus(m_boundsMin).times(0.5);
// calculate max z
pyramidTop.z() = std::max(
(m_cameraMax.x() - m_cameraMin.x())/(2*std::tan(camera.getHorizontalFov()/2.0)),
(m_cameraMax.y() - m_cameraMin.y())/(2*std::tan(camera.getVerticalFov()/2.0))
) + m_minimumCameraZ;
if (newCamLocation.z() > pyramidTop.z()) {
newCamLocation = pyramidTop;
}
else {
// up dir
meters upBase = (m_cameraMax.y() - m_cameraMin.y()) / 2.0;
meters upMax = pyramidTop.y()
+ upBase*(pyramidTop.z() - newCamLocation.z())
/ (pyramidTop.z() - m_minimumCameraZ);
if ( newCamLocation.y() - pyramidTop.y() > upMax) {
newCamLocation.y() = upMax + pyramidTop.y();
}
else if ( newCamLocation.y() - pyramidTop.y() < -upMax ) {
newCamLocation.y() = - upMax + pyramidTop.y();
}
// right dir
meters rightBase = (m_cameraMax.x() - m_cameraMin.x()) / 2.0;
meters rightMax = pyramidTop.x()
+ rightBase*(pyramidTop.z() - newCamLocation.z())
/ (pyramidTop.z() - m_minimumCameraZ);
if ( newCamLocation.x() - pyramidTop.x() > rightMax) {
newCamLocation.x() = rightMax + pyramidTop.x();
}
else if ( newCamLocation.x() - pyramidTop.x() < -rightMax ) {
newCamLocation.x() = - rightMax + pyramidTop.x();
}
}
camera.setLocation(newCamLocation);
// adjust velocity
camVelocity = camVelocity.times(1 - m_cameraFriction);
if (camVelocity.magnitude() < 0.1) {
camVelocity = Vector3D(0,0,0);
}
}
m_boundsMin = Vector3D( MAX_METERS, MAX_METERS, MAX_METERS);
m_boundsMax = Vector3D(-MAX_METERS,-MAX_METERS,-MAX_METERS);
m_cameraMin = Vector3D( MAX_METERS, MAX_METERS, MAX_METERS);
m_cameraMax = Vector3D(-MAX_METERS,-MAX_METERS,-MAX_METERS);
}
double ParticleConstraint::currentLength() const
{
Vector3D relativePos = particle->getPosition() - anchor;
return relativePos.magnitude();
}
double ParticleLink::currentLength() const
{
Vector3D relativePos = particle[0]->getPosition() -
particle[1]->getPosition();
return relativePos.magnitude();
}
//------------------------------------------------------------------------------
//! Computes the Jacobian for all Edges Internal and Boundary
//! Note: Jacobian is computed using first order Q's
//------------------------------------------------------------------------------
void Compute_Jacobian_Approximate_Roe(int AddTime, int Iteration) {
int i, j, k, iNode, iEdge, ibEdge;
int node_L, node_R;
int idgn, idgnL, idgnR, ofdgnL, ofdgnR;
double area;
double Q_L[NEQUATIONS];
double Q_R[NEQUATIONS];
double **dFdL;
double **dFdR;
double **ARoe;
Vector3D areavec;
// Create the Helper Matrix
dFdL = (double **) malloc(NEQUATIONS*sizeof(double*));
dFdR = (double **) malloc(NEQUATIONS*sizeof(double*));
ARoe = (double **) malloc(NEQUATIONS*sizeof(double*));
for (i = 0; i < NEQUATIONS; i++) {
dFdL[i] = (double *) malloc(NEQUATIONS*sizeof(double));
dFdR[i] = (double *) malloc(NEQUATIONS*sizeof(double));
ARoe[i] = (double *) malloc(NEQUATIONS*sizeof(double));
}
// Initialize the CRS Matrix
for (i = 0; i < SolverBlockMatrix.DIM; i++) {
for (j = 0; j < SolverBlockMatrix.Block_nRow; j++) {
for (k = 0; k < SolverBlockMatrix.Block_nCol; k++)
SolverBlockMatrix.A[i][j][k] = 0.0;
}
}
// Copy the Residuals to Block Matrix which is B
// And Copy I/DeltaT
for (iNode = 0; iNode < nNode; iNode++) {
// Get the diagonal location
idgn = SolverBlockMatrix.IAU[iNode];
// Get the LHS
SolverBlockMatrix.B[iNode][0] = -Res1[iNode];
SolverBlockMatrix.B[iNode][1] = -Res2[iNode];
SolverBlockMatrix.B[iNode][2] = -Res3[iNode];
SolverBlockMatrix.B[iNode][3] = -Res4[iNode];
SolverBlockMatrix.B[iNode][4] = -Res5[iNode];
for (j = 0; j < SolverBlockMatrix.Block_nRow; j++) {
if (AddTime == 1) {
for (k = 0; k < SolverBlockMatrix.Block_nCol; k++)
if (k == j)
SolverBlockMatrix.A[idgn][j][k] = cVolume[iNode]/DeltaT[iNode];
}
}
}
// Internal Edges
for (iEdge = 0; iEdge < nEdge; iEdge++) {
// Get two nodes of edge
node_L = intEdge[iEdge].node[0];
node_R = intEdge[iEdge].node[1];
// Get area vector
areavec = intEdge[iEdge].areav;
area = areavec.magnitude();
// Backup the Copy of Q_L and Q_R
// Left
Q_L[0] = Q1[node_L];
Q_L[1] = Q2[node_L];
Q_L[2] = Q3[node_L];
Q_L[3] = Q4[node_L];
Q_L[4] = Q5[node_L];
// Right
Q_R[0] = Q1[node_R];
Q_R[1] = Q2[node_R];
Q_R[2] = Q3[node_R];
Q_R[3] = Q4[node_R];
Q_R[4] = Q5[node_R];
// Get the diagonal Locations
idgnL = SolverBlockMatrix.IAU[node_L];
idgnR = SolverBlockMatrix.IAU[node_R];
// Get the Off-Diagonal Locations
// node_L: ofdgnL-> node_R;
// node_R: ofdgnR-> node_L;
ofdgnL = -1;
for (i = SolverBlockMatrix.IA[node_L]; i < SolverBlockMatrix.IA[node_L+1]; i++) {
if (SolverBlockMatrix.JA[i] == node_R) {
ofdgnL = i;
break;
}
}
ofdgnR = -1;
for (i = SolverBlockMatrix.IA[node_R]; i < SolverBlockMatrix.IA[node_R+1]; i++) {
if (SolverBlockMatrix.JA[i] == node_L) {
ofdgnR = i;
break;
}
}
// Initialize the Helper Matrix
for (i = 0; i < NEQUATIONS; i++) {
for (j = 0; j < NEQUATIONS; j++) {
dFdL[i][j] = 0.0;
dFdR[i][j] = 0.0;
ARoe[i][j] = 0.0;
}
}
// Compute dFdL
ConservativeEulerFluxJacobian(Q_L, areavec, dFdL, Gamma);
// Compute dFdR
ConservativeEulerFluxJacobian(Q_R, areavec, dFdR, Gamma);
// Compute Roe Jacobian
Compute_RoeAJacobian(Q_L, Q_R, areavec, ARoe);
// Finally Compute the dFlux/dQ_L and dFlux/dQ_R
for (i = 0; i < NEQUATIONS; i++) {
for (j = 0; j < NEQUATIONS; j++) {
dFdL[i][j] = 0.5*(dFdL[i][j] + ARoe[i][j])*area;
dFdR[i][j] = 0.5*(dFdR[i][j] - ARoe[i][j])*area;
}
}
// Update the Diagonal and Off-Diagonal Terms
for (i = 0; i < NEQUATIONS; i++) {
for (j = 0; j < NEQUATIONS; j++) {
// Diagonal
SolverBlockMatrix.A[idgnL][i][j] += dFdL[i][j];
SolverBlockMatrix.A[idgnR][i][j] -= dFdR[i][j];
// Off-Diagonal
SolverBlockMatrix.A[ofdgnL][i][j] += dFdR[i][j];
SolverBlockMatrix.A[ofdgnR][i][j] -= dFdL[i][j];
}
}
}
// Boundary Edges
for (ibEdge = 0; ibEdge < nBEdge; ibEdge++) {
// Get two nodes of edge
node_L = bndEdge[ibEdge].node[0];
node_R = bndEdge[ibEdge].node[1];
// Get area vector
areavec = bndEdge[ibEdge].areav;
area = areavec.magnitude();
// Backup the Copy of Q_L and Q_R
// Left - Physical
Q_L[0] = Q1[node_L];
Q_L[1] = Q2[node_L];
Q_L[2] = Q3[node_L];
Q_L[3] = Q4[node_L];
Q_L[4] = Q5[node_L];
// Right - Ghost
Q_R[0] = Q1[node_R];
Q_R[1] = Q2[node_R];
Q_R[2] = Q3[node_R];
Q_R[3] = Q4[node_R];
Q_R[4] = Q5[node_R];
// Get the diagonal Locations - Only Physical
// No diagonal and off-diagonal locations exists for Ghost Nodes
idgnL = SolverBlockMatrix.IAU[node_L];
// Initialize the Helper Matrix - Only Physical
for (i = 0; i < NEQUATIONS; i++) {
for (j = 0; j < NEQUATIONS; j++) {
dFdL[i][j] = 0.0;
ARoe[i][j] = 0.0;
}
}
// Compute dFdL
ConservativeEulerFluxJacobian(Q_L, areavec, dFdL, Gamma);
// Compute Roe Jacobian
Compute_RoeAJacobian(Q_L, Q_R, areavec, ARoe);
// Finally Compute the dFlux/dQ_L
for (i = 0; i < NEQUATIONS; i++) {
for (j = 0; j < NEQUATIONS; j++)
dFdL[i][j] = 0.5*(dFdL[i][j] + ARoe[i][j])*area;
}
// Update the Diagonal Term of Physical Node Only
for (i = 0; i < NEQUATIONS; i++) {
for (j = 0; j < NEQUATIONS; j++)
SolverBlockMatrix.A[idgnL][i][j] += dFdL[i][j];
}
}
// Delete the Helper Matrix
for (i = 0; i < NEQUATIONS; i++) {
free(ARoe[i]);
free(dFdL[i]);
free(dFdR[i]);
}
free(ARoe);
free(dFdL);
free(dFdR);
}
float Vector3D::theta(Vector3D& v)
{
float t = acos(this->dotProduct(v) / (this->magnitude() * v.magnitude()));
return t; // t is in radians
}
