Exemplo n.º 1
0
// 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;
}
Exemplo n.º 2
0
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);
  } 
}
Exemplo n.º 3
0
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));
}
Exemplo n.º 4
0
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);
}
Exemplo n.º 5
0
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);
}
Exemplo n.º 6
0
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;
}
Exemplo n.º 7
0
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;
}
Exemplo n.º 8
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;
}
Exemplo n.º 9
0
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;
            }
        }
    }
}
Exemplo n.º 11
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;
  }
}
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
0
Vector3D Triangle3::faceNormal() const {
    Vector3D ret = (points[1] - points[0]).cross(points[2] - points[0]);
    return ret.normalized();
}
Exemplo n.º 14
0
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;
        }
    }