/** * 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 }