// static method implementation Matrix4x4 Matrix4x4::rotate(const double angleInRad, const Vector3D &axis) { Vector3D a = axis.normalized(); double s = std::sin(angleInRad); double c = std::cos(angleInRad); double m[4][4]; m[0][0] = a.x * a.x + (1.0 - a.x * a.x) * c; m[0][1] = a.x * a.y * (1.0 - c) - a.z * s; m[0][2] = a.x * a.z * (1.0 - c) + a.y * s; m[0][3] = 0; m[1][0] = a.x * a.y * (1.0 - c) + a.z * s; m[1][1] = a.y * a.y + (1.0 - a.y * a.y) * c; m[1][2] = a.y * a.z * (1.0 - c) - a.x * s; m[1][3] = 0; m[2][0] = a.x * a.z * (1.0 - c) - a.y * s; m[2][1] = a.y * a.z * (1.0 - c) + a.x * s; m[2][2] = a.z * a.z + (1.0 - a.z * a.z) * c; m[2][3] = 0; m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; Matrix4x4 mat(m); return mat; }
void CameraRotationHandler::onMove(const G3MEventContext *eventContext, const TouchEvent& touchEvent, CameraContext *cameraContext) { const IMathUtils* mu = IMathUtils::instance(); //_currentGesture = getGesture(touchEvent); if (cameraContext->getCurrentGesture() != Rotate) return; // current middle pixel in 2D const Vector2I c0 = touchEvent.getTouch(0)->getPos(); const Vector2I c1 = touchEvent.getTouch(1)->getPos(); const Vector2I c2 = touchEvent.getTouch(2)->getPos(); const Vector2I cm = c0.add(c1).add(c2).div(3); // compute normal to Initial point Vector3D normal = eventContext->getPlanet()->geodeticSurfaceNormal(_pivotPoint ); // vertical rotation around normal vector to globe Camera *camera = cameraContext->getNextCamera(); camera->copyFrom(_camera0); Angle angle_v = Angle::fromDegrees((_pivotPixel.x()-cm._x)*0.25); camera->rotateWithAxisAndPoint(normal, _pivotPoint.asVector3D(), angle_v); // compute angle between normal and view direction Vector3D view = camera->getViewDirection(); double dot = normal.normalized().dot(view.normalized().times(-1)); double initialAngle = mu->acos(dot) / mu->pi() * 180; // rotate more than 85 degrees or less than 0 degrees is not allowed double delta = (cm._y - _pivotPixel.y()) * 0.25; double finalAngle = initialAngle + delta; if (finalAngle > 85) delta = 85 - initialAngle; if (finalAngle < 0) delta = -initialAngle; // create temporal camera to test if next rotation is valid Camera tempCamera(*camera); // horizontal rotation over the original camera horizontal axix Vector3D u = camera->getHorizontalVector(); tempCamera.rotateWithAxisAndPoint(u, _pivotPoint.asVector3D(), Angle::fromDegrees(delta)); // update camera only if new view intersects globe //tempCamera.updateModelMatrix(); if (!tempCamera.getXYZCenterOfView().isNan()) { camera->copyFrom(tempCamera); } }
Angle Vector3D::angleBetween(const Vector3D& other) const { const Vector3D v1 = normalized(); const Vector3D v2 = other.normalized(); double c = v1.dot(v2); if (c > 1.0) c = 1.0; else if (c < -1.0) c = -1.0; return Angle::fromRadians(IMathUtils::instance()->acos(c)); }
void testProjectionAlgorithms( const Vector3D& source, const Vector3D& target) { Vector3D targetNormalized = target.normalized(); Vector3D oldResult = source.dot(targetNormalized) * targetNormalized; Vector3D newResult = source.projectOnto(target); EXPECT_FLOAT_EQ(oldResult.x, newResult.x); EXPECT_FLOAT_EQ(oldResult.y, newResult.y); EXPECT_FLOAT_EQ(oldResult.z, newResult.z); }
MutableMatrix44D MutableMatrix44D::createRotationMatrix(const Angle& angle, const Vector3D& axis) { const Vector3D a = axis.normalized(); const double c = angle.cosinus(); const double s = angle.sinus(); return MutableMatrix44D(a._x * a._x * (1 - c) + c, a._x * a._y * (1 - c) + a._z * s, a._x * a._z * (1 - c) - a._y * s, 0, a._y * a._x * (1 - c) - a._z * s, a._y * a._y * (1 - c) + c, a._y * a._z * (1 - c) + a._x * s, 0, a._x * a._z * (1 - c) + a._y * s, a._y * a._z * (1 - c) - a._x * s, a._z * a._z * (1 - c) + c, 0, 0, 0, 0, 1); }
SurfaceRayIntersection3 Triangle3::closestIntersectionLocal( const Ray3D& ray) const { SurfaceRayIntersection3 intersection; Vector3D n = faceNormal(); double nd = n.dot(ray.direction); if (nd < std::numeric_limits<double>::epsilon()) { intersection.isIntersecting = false; return intersection; } double d = n.dot(points[0]); double t = (d - n.dot(ray.origin)) / nd; if (t < 0.0) { intersection.isIntersecting = false; return intersection; } Vector3D q = ray.pointAt(t); Vector3D q01 = (points[1] - points[0]).cross(q - points[0]); if (n.dot(q01) <= 0.0) { intersection.isIntersecting = false; return intersection; } Vector3D q12 = (points[2] - points[1]).cross(q - points[1]); if (n.dot(q12) <= 0.0) { intersection.isIntersecting = false; return intersection; } Vector3D q02 = (points[0] - points[2]).cross(q - points[2]); if (n.dot(q02) <= 0.0) { intersection.isIntersecting = false; return intersection; } double a = area(); double b0 = 0.5 * q12.length() / a; double b1 = 0.5 * q02.length() / a; double b2 = 0.5 * q01.length() / a; Vector3D normal = b0 * normals[0] + b1 * normals[1] + b2 * normals[2]; intersection.isIntersecting = true; intersection.distance = t; intersection.point = q; intersection.normal = normal.normalized(); return intersection; }
int main(int , char** ) { for (int k = 0; k < 100000; ++k) { // create a random rotation matrix by sampling a random 3d vector // that will be used in axis-angle representation to create the matrix Vector3D rotAxisAngle = Vector3D::Random(); rotAxisAngle += Vector3D::Random(); Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized()); Matrix3D Re = rotation.toRotationMatrix(); // our analytic function which we want to evaluate Matrix<double, 3, 9, Eigen::ColMajor> dq_dR; compute_dq_dR (dq_dR, Re(0,0),Re(1,0),Re(2,0), Re(0,1),Re(1,1),Re(2,1), Re(0,2),Re(1,2),Re(2,2)); // compute the Jacobian using AD Matrix<double, 3, 9, Eigen::RowMajor> dq_dR_AD; typedef ceres::internal::AutoDiff<RotationMatrix2QuaternionManifold, double, 9> AutoDiff_Dq_DR; double *parameters[] = { Re.data() }; double *jacobians[] = { dq_dR_AD.data() }; double value[3]; RotationMatrix2QuaternionManifold rot2quat; AutoDiff_Dq_DR::Differentiate(rot2quat, parameters, 3, value, jacobians); // compare the two Jacobians const double allowedDifference = 1e-6; for (int i = 0; i < dq_dR.rows(); ++i) { for (int j = 0; j < dq_dR.cols(); ++j) { double d = fabs(dq_dR_AD(i,j) - dq_dR(i,j)); if (d > allowedDifference) { cerr << "\ndetected difference in the Jacobians" << endl; cerr << PVAR(Re) << endl << endl; cerr << PVAR(dq_dR_AD) << endl << endl; cerr << PVAR(dq_dR) << endl << endl; return 1; } } } cerr << "+"; } return 0; }
bool Walls::collided(Vector3D shipPosition) { bool collided = false; const unsigned int NUM_WALLPOINTS = 4; for(unsigned int i = 0; i< NUM_WALLPOINTS; i++) { Vector3D wallVertexOne = *wallShape[i]; Vector3D wallToShip = shipPosition - wallVertexOne; Vector3D wallVertexTwo = *wallShape[(i+1) % NUM_WALLPOINTS]; Vector3D wall = wallVertexTwo - wallVertexOne; Vector3D wallNormal = wall.perpCW(); float dotResult = dot(wallToShip, wallNormal); if(dotResult<0) { collided = true; collidedNormal = wallNormal.normalized(); } } return collided; }
void GraphicalArrow3D::setValue(Vector3D base, Vector3D normal, double length) { // find an arbitrary vector k perpendicular to normal: Vector3D k = cross(Vector3D(1,0,0), normal); if (similar(k, Vector3D(0,0,0))) k = cross(Vector3D(0,1,0), normal); k = k.normalized(); Transform3D M = createTransformIJC(normal, k, base); // std::cout << "GraphicalArrow3D::setValue " << base << " - " << normal << std::endl; Transform3D S = createTransformScale(Vector3D(length,1,1)); M = M * S; // let arrow shape increase slowly with length: // source->SetTipLength(0.35/sqrt(length)); // source->SetTipRadius(0.1*sqrt(length)); // source->SetShaftRadius(0.03*sqrt(length)); source->SetTipLength(0.35); source->SetTipRadius(0.1*(length)); source->SetShaftRadius(0.03*(length)); actor->SetUserMatrix(M.getVtkMatrix()); }
void GridFractionalBoundaryConditionSolver3::constrainVelocity( FaceCenteredGrid3* velocity, unsigned int extrapolationDepth) { Size3 size = velocity->resolution(); if (_colliderSdf == nullptr || _colliderSdf->resolution() != size) { updateCollider( collider(), size, velocity->gridSpacing(), velocity->origin()); } auto u = velocity->uAccessor(); auto v = velocity->vAccessor(); auto w = velocity->wAccessor(); auto uPos = velocity->uPosition(); auto vPos = velocity->vPosition(); auto wPos = velocity->wPosition(); Array3<double> uTemp(u.size()); Array3<double> vTemp(v.size()); Array3<double> wTemp(w.size()); Array3<char> uMarker(u.size(), 1); Array3<char> vMarker(v.size(), 1); Array3<char> wMarker(w.size(), 1); Vector3D h = velocity->gridSpacing(); // Assign collider's velocity first and initialize markers velocity->parallelForEachUIndex([&](size_t i, size_t j, size_t k) { Vector3D pt = uPos(i, j, k); double phi0 = _colliderSdf->sample(pt - Vector3D(0.5 * h.x, 0.0, 0.0)); double phi1 = _colliderSdf->sample(pt + Vector3D(0.5 * h.x, 0.0, 0.0)); double frac = fractionInsideSdf(phi0, phi1); frac = 1.0 - clamp(frac, 0.0, 1.0); if (frac > 0.0) { uMarker(i, j, k) = 1; } else { Vector3D colliderVel = collider()->velocityAt(pt); u(i, j, k) = colliderVel.x; uMarker(i, j, k) = 0; } }); velocity->parallelForEachVIndex([&](size_t i, size_t j, size_t k) { Vector3D pt = vPos(i, j, k); double phi0 = _colliderSdf->sample(pt - Vector3D(0.0, 0.5 * h.y, 0.0)); double phi1 = _colliderSdf->sample(pt + Vector3D(0.0, 0.5 * h.y, 0.0)); double frac = fractionInsideSdf(phi0, phi1); frac = 1.0 - clamp(frac, 0.0, 1.0); if (frac > 0.0) { vMarker(i, j, k) = 1; } else { Vector3D colliderVel = collider()->velocityAt(pt); v(i, j, k) = colliderVel.y; vMarker(i, j, k) = 0; } }); velocity->parallelForEachWIndex([&](size_t i, size_t j, size_t k) { Vector3D pt = wPos(i, j, k); double phi0 = _colliderSdf->sample(pt - Vector3D(0.0, 0.0, 0.5 * h.z)); double phi1 = _colliderSdf->sample(pt + Vector3D(0.0, 0.0, 0.5 * h.z)); double frac = fractionInsideSdf(phi0, phi1); frac = 1.0 - clamp(frac, 0.0, 1.0); if (frac > 0.0) { wMarker(i, j, k) = 1; } else { Vector3D colliderVel = collider()->velocityAt(pt); w(i, j, k) = colliderVel.z; wMarker(i, j, k) = 0; } }); // Free-slip: Extrapolate fluid velocity into the collider extrapolateToRegion( velocity->uConstAccessor(), uMarker, extrapolationDepth, u); extrapolateToRegion( velocity->vConstAccessor(), vMarker, extrapolationDepth, v); extrapolateToRegion( velocity->wConstAccessor(), wMarker, extrapolationDepth, w); // No-flux: project the extrapolated velocity to the collider's surface // normal velocity->parallelForEachUIndex([&](size_t i, size_t j, size_t k) { Vector3D pt = uPos(i, j, k); if (isInsideSdf(_colliderSdf->sample(pt))) { Vector3D colliderVel = collider()->velocityAt(pt); Vector3D vel = velocity->sample(pt); Vector3D g = _colliderSdf->gradient(pt); if (g.lengthSquared() > 0.0) { Vector3D n = g.normalized(); Vector3D velr = vel - colliderVel; Vector3D velt = projectAndApplyFriction( velr, n, collider()->frictionCoefficient()); Vector3D velp = velt + colliderVel; uTemp(i, j, k) = velp.x; } else { uTemp(i, j, k) = colliderVel.x; } } else { uTemp(i, j, k) = u(i, j, k); } }); velocity->parallelForEachVIndex([&](size_t i, size_t j, size_t k) { Vector3D pt = vPos(i, j, k); if (isInsideSdf(_colliderSdf->sample(pt))) { Vector3D colliderVel = collider()->velocityAt(pt); Vector3D vel = velocity->sample(pt); Vector3D g = _colliderSdf->gradient(pt); if (g.lengthSquared() > 0.0) { Vector3D n = g.normalized(); Vector3D velr = vel - colliderVel; Vector3D velt = projectAndApplyFriction( velr, n, collider()->frictionCoefficient()); Vector3D velp = velt + colliderVel; vTemp(i, j, k) = velp.y; } else { vTemp(i, j, k) = colliderVel.y; } } else { vTemp(i, j, k) = v(i, j, k); } }); velocity->parallelForEachWIndex([&](size_t i, size_t j, size_t k) { Vector3D pt = wPos(i, j, k); if (isInsideSdf(_colliderSdf->sample(pt))) { Vector3D colliderVel = collider()->velocityAt(pt); Vector3D vel = velocity->sample(pt); Vector3D g = _colliderSdf->gradient(pt); if (g.lengthSquared() > 0.0) { Vector3D n = g.normalized(); Vector3D velr = vel - colliderVel; Vector3D velt = projectAndApplyFriction( velr, n, collider()->frictionCoefficient()); Vector3D velp = velt + colliderVel; wTemp(i, j, k) = velp.z; } else { wTemp(i, j, k) = colliderVel.z; } } else { wTemp(i, j, k) = w(i, j, k); } }); // Transfer results u.parallelForEachIndex([&](size_t i, size_t j, size_t k) { u(i, j, k) = uTemp(i, j, k); }); v.parallelForEachIndex([&](size_t i, size_t j, size_t k) { v(i, j, k) = vTemp(i, j, k); }); w.parallelForEachIndex([&](size_t i, size_t j, size_t k) { w(i, j, k) = wTemp(i, j, k); }); // No-flux: Project velocity on the domain boundary if closed if (closedDomainBoundaryFlag() & kDirectionLeft) { for (size_t k = 0; k < u.size().z; ++k) { for (size_t j = 0; j < u.size().y; ++j) { u(0, j, k) = 0; } } } if (closedDomainBoundaryFlag() & kDirectionRight) { for (size_t k = 0; k < u.size().z; ++k) { for (size_t j = 0; j < u.size().y; ++j) { u(u.size().x - 1, j, k) = 0; } } } if (closedDomainBoundaryFlag() & kDirectionDown) { for (size_t k = 0; k < v.size().z; ++k) { for (size_t i = 0; i < v.size().x; ++i) { v(i, 0, k) = 0; } } } if (closedDomainBoundaryFlag() & kDirectionUp) { for (size_t k = 0; k < v.size().z; ++k) { for (size_t i = 0; i < v.size().x; ++i) { v(i, v.size().y - 1, k) = 0; } } } if (closedDomainBoundaryFlag() & kDirectionBack) { for (size_t j = 0; j < w.size().y; ++j) { for (size_t i = 0; i < w.size().x; ++i) { w(i, j, 0) = 0; } } } if (closedDomainBoundaryFlag() & kDirectionFront) { for (size_t j = 0; j < w.size().y; ++j) { for (size_t i = 0; i < w.size().x; ++i) { w(i, j, w.size().z - 1) = 0; } } } }
void HQRenderer::mouseMove_(int x, int y, int buttons){ int width = CGE::Engine::instance()->getWindow()->getWidth(); int height = CGE::Engine::instance()->getWindow()->getHeight(); //if (x == width/2 && y == height/2) // return; if(threeD_ && !Mouse::instance()->isMouseActive()){ float angleY = 0.0f; float angleZ = 0.0f; int xrel = x - mousePos_.x; int yrel = y - mousePos_.y; //if (xrel == 0 && yrel == 0) // return; //too unprecise to set it every move //mousePos_.x = width/2; //mousePos_.y = height/2; //Mouse::instance()->setMousePos(width/2, height/2); bool needsSet = false; if (x < 10){ x = width - 10; needsSet = true; } else if (x > width - 10){ x = 10; needsSet = true; } if (y < 10){ y = height - 10; needsSet = true; } else if (y > height - 10){ y = 10; needsSet = true; } if (needsSet) Mouse::instance()->setMousePos(x, y); mousePos_.x = x; mousePos_.y = y; angleY = (float) (-xrel)/400.0f; angleZ = (float) (-yrel)/400.0f; cam.setLastRotX(cam.getCurrRotX()); cam.setCurrRotX(cam.getCurrRotX()+angleZ); Vector3D look = cam.view(); Vector3D pos = cam.position(); Vector3D up = cam.upVector(); Vector3D axis = (look - pos).cross(up); axis = axis.normalized(); //stop camera from going too high or too low if (cam.getCurrRotX() > 1.0){ cam.setCurrRotX(1.0); if (cam.getLastRotX() != 1.0f){ cam.rotateView(1.0f - cam.getLastRotX(), axis); } } else if (cam.getCurrRotX() < -1.0){ cam.setCurrRotX(-1.0); if (cam.getLastRotX() != -1.0f){ cam.rotateView(-1.0f - cam.getLastRotX(), axis); } } else{ cam.rotateView(angleZ, axis); } cam.rotateView(angleY, Vector3D(0.0, 1.0, 0.0)); } else{ mousePos_.x = x; mousePos_.y = y; } }
MutableMatrix44D SphericalPlanet::doubleDrag(const Vector3D& finalRay0, const Vector3D& finalRay1) const { // test if initialPoints are valid if (_initialPoint0.isNan() || _initialPoint1.isNan()) return MutableMatrix44D::invalid(); // init params const IMathUtils* mu = IMathUtils::instance(); MutableVector3D positionCamera = _origin; const double finalRaysAngle = finalRay0.angleBetween(finalRay1)._degrees; const double factor = finalRaysAngle / _angleBetweenInitialRays; double dAccum=0, angle0, angle1; double distance = _origin.sub(_centerPoint).length(); // compute estimated camera translation: step 0 double d = distance*(factor-1)/factor; MutableMatrix44D translation = MutableMatrix44D::createTranslationMatrix(_centerRay.asVector3D().normalized().times(d)); positionCamera = positionCamera.transformedBy(translation, 1.0); dAccum += d; { const Vector3D point0 = closestIntersection(positionCamera.asVector3D(), finalRay0); const Vector3D point1 = closestIntersection(positionCamera.asVector3D(), finalRay1); angle0 = point0.angleBetween(point1)._degrees; if (ISNAN(angle0)) return MutableMatrix44D::invalid(); } // compute estimated camera translation: step 1 d = mu->abs((distance-d)*0.3); if (angle0 < _angleBetweenInitialPoints) d*=-1; translation = MutableMatrix44D::createTranslationMatrix(_centerRay.asVector3D().normalized().times(d)); positionCamera = positionCamera.transformedBy(translation, 1.0); dAccum += d; { const Vector3D point0 = closestIntersection(positionCamera.asVector3D(), finalRay0); const Vector3D point1 = closestIntersection(positionCamera.asVector3D(), finalRay1); angle1 = point0.angleBetween(point1)._degrees; if (ISNAN(angle1)) return MutableMatrix44D::invalid(); } // compute estimated camera translation: steps 2..n until convergence //int iter=0; double precision = mu->pow(10, mu->log10(distance)-8.0); double angle_n1=angle0, angle_n=angle1; while (mu->abs(angle_n-_angleBetweenInitialPoints) > precision) { // iter++; if ((angle_n1-angle_n)/(angle_n-_angleBetweenInitialPoints) < 0) d*=-0.5; translation = MutableMatrix44D::createTranslationMatrix(_centerRay.asVector3D().normalized().times(d)); positionCamera = positionCamera.transformedBy(translation, 1.0); dAccum += d; angle_n1 = angle_n; { const Vector3D point0 = closestIntersection(positionCamera.asVector3D(), finalRay0); const Vector3D point1 = closestIntersection(positionCamera.asVector3D(), finalRay1); angle_n = point0.angleBetween(point1)._degrees; if (ISNAN(angle_n)) return MutableMatrix44D::invalid(); } } //if (iter>2) printf("----------- iteraciones=%d precision=%f angulo final=%.4f distancia final=%.1f\n", iter, precision, angle_n, dAccum); // start to compound matrix MutableMatrix44D matrix = MutableMatrix44D::identity(); positionCamera = _origin; MutableVector3D viewDirection = _centerRay; MutableVector3D ray0 = finalRay0.asMutableVector3D(); MutableVector3D ray1 = finalRay1.asMutableVector3D(); // drag from initialPoint to centerPoint { Vector3D initialPoint = _initialPoint.asVector3D(); const Vector3D rotationAxis = initialPoint.cross(_centerPoint.asVector3D()); const Angle rotationDelta = Angle::fromRadians( - mu->acos(_initialPoint.normalized().dot(_centerPoint.normalized())) ); if (rotationDelta.isNan()) return MutableMatrix44D::invalid(); MutableMatrix44D rotation = MutableMatrix44D::createRotationMatrix(rotationDelta, rotationAxis); positionCamera = positionCamera.transformedBy(rotation, 1.0); viewDirection = viewDirection.transformedBy(rotation, 0.0); ray0 = ray0.transformedBy(rotation, 0.0); ray1 = ray1.transformedBy(rotation, 0.0); matrix = rotation.multiply(matrix); } // move the camera forward { MutableMatrix44D translation2 = MutableMatrix44D::createTranslationMatrix(viewDirection.asVector3D().normalized().times(dAccum)); positionCamera = positionCamera.transformedBy(translation2, 1.0); matrix = translation2.multiply(matrix); } // compute 3D point of view center Vector3D centerPoint2 = closestIntersection(positionCamera.asVector3D(), viewDirection.asVector3D()); // compute middle point in 3D Vector3D P0 = closestIntersection(positionCamera.asVector3D(), ray0.asVector3D()); Vector3D P1 = closestIntersection(positionCamera.asVector3D(), ray1.asVector3D()); Geodetic2D g = getMidPoint(toGeodetic2D(P0), toGeodetic2D(P1)); Vector3D finalPoint = toCartesian(g); // drag globe from centerPoint to finalPoint { const Vector3D rotationAxis = centerPoint2.cross(finalPoint); const Angle rotationDelta = Angle::fromRadians( - mu->acos(centerPoint2.normalized().dot(finalPoint.normalized())) ); if (rotationDelta.isNan()) return MutableMatrix44D::invalid(); MutableMatrix44D rotation = MutableMatrix44D::createRotationMatrix(rotationDelta, rotationAxis); positionCamera = positionCamera.transformedBy(rotation, 1.0); viewDirection = viewDirection.transformedBy(rotation, 0.0); ray0 = ray0.transformedBy(rotation, 0.0); ray1 = ray1.transformedBy(rotation, 0.0); matrix = rotation.multiply(matrix); } // camera rotation { Vector3D normal = geodeticSurfaceNormal(centerPoint2); Vector3D v0 = _initialPoint0.asVector3D().sub(centerPoint2).projectionInPlane(normal); Vector3D p0 = closestIntersection(positionCamera.asVector3D(), ray0.asVector3D()); Vector3D v1 = p0.sub(centerPoint2).projectionInPlane(normal); double angle = v0.angleBetween(v1)._degrees; double sign = v1.cross(v0).dot(normal); if (sign<0) angle = -angle; MutableMatrix44D rotation = MutableMatrix44D::createGeneralRotationMatrix(Angle::fromDegrees(angle), normal, centerPoint2); matrix = rotation.multiply(matrix); } return matrix; }
Vector3D Triangle3::faceNormal() const { Vector3D ret = (points[1] - points[0]).cross(points[2] - points[0]); return ret.normalized(); }
Ray::Ray(const Vector3D& o, const Vector3D& d):origine(o),direction(d.normalized()) { }
void onAdvanceTimeStep(double timeIntervalInSeconds) override { size_t numberOfPoints = positions.size(); size_t numberOfEdges = edges.size(); // Compute forces for (size_t i = 0; i < numberOfPoints; ++i) { // Gravity force forces[i] = mass * gravity; // Air drag force Vector3D relativeVel = velocities[i]; if (wind != nullptr) { relativeVel -= wind->sample(positions[i]); } forces[i] += -dragCoefficient * relativeVel; } for (size_t i = 0; i < numberOfEdges; ++i) { size_t pointIndex0 = edges[i].first; size_t pointIndex1 = edges[i].second; // Compute spring force Vector3D pos0 = positions[pointIndex0]; Vector3D pos1 = positions[pointIndex1]; Vector3D r = pos0 - pos1; double distance = r.length(); if (distance > 0.0) { Vector3D force = -stiffness * (distance - restLength) * r.normalized(); forces[pointIndex0] += force; forces[pointIndex1] -= force; } // Add damping force Vector3D vel0 = velocities[pointIndex0]; Vector3D vel1 = velocities[pointIndex1]; Vector3D relativeVel0 = vel0 - vel1; Vector3D damping = -dampingCoefficient * relativeVel0; forces[pointIndex0] += damping; forces[pointIndex1] -= damping; } // Update states for (size_t i = 0; i < numberOfPoints; ++i) { // Compute new states Vector3D newAcceleration = forces[i] / mass; Vector3D newVelocity = velocities[i] + timeIntervalInSeconds * newAcceleration; Vector3D newPosition = positions[i] + timeIntervalInSeconds * newVelocity; // Collision if (newPosition.y < floorPositionY) { newPosition.y = floorPositionY; if (newVelocity.y < 0.0) { newVelocity.y *= -restitutionCoefficient; newPosition.y += timeIntervalInSeconds * newVelocity.y; } } // Update states velocities[i] = newVelocity; positions[i] = newPosition; } // Apply constraints for (size_t i = 0; i < constraints.size(); ++i) { size_t pointIndex = constraints[i].pointIndex; positions[pointIndex] = constraints[i].fixedPosition; velocities[pointIndex] = constraints[i].fixedVelocity; } }