1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586 |
- // Include own headers
- #include "TrackPoint.hpp"
- // Include dependencies
- #include <osg/Geode>
- #include <osg/ShapeDrawable>
- #include <osg/Material>
- #include <osg/StateSet>
- TrackPoint::TrackPoint(osg::Vec3 point, osg::Vec3 normal) {
- osg::ref_ptr<osg::Geode> geode = new osg::Geode;
- osg::ref_ptr<osg::ShapeDrawable> cylinder = new osg::ShapeDrawable();
- cylinder->setShape(new osg::Cylinder(osg::Vec3(0.0f, 0.0f, 0.0f), 1.0f, 10.0f));
- cylinder->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 1.0f));
- geode->addDrawable(cylinder.get());
- {
- osg::StateSet* stateSet = geode->getOrCreateStateSet();
- osg::Material* material = new osg::Material;
- material->setColorMode(osg::Material::AMBIENT_AND_DIFFUSE);
- stateSet->setAttributeAndModes(material, osg::StateAttribute::ON);
- stateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
- }
- _rotationGroup = new osg::MatrixTransform;
- _rotationGroup->addChild(geode.get());
- _rotationGroup->setMatrix(osg::Matrix::rotate(osg::Vec3f(0.0f, 0.0f, 1.0f), normal));
- _originFixGroup = new osg::MatrixTransform;
- _originFixGroup->addChild(_rotationGroup.get());
- osg::Vec3f movementVector = normal.operator*(5.0f);
- _originFixGroup->setMatrix(osg::Matrix::translate(movementVector));
- _translationGroup = new osg::MatrixTransform;
- _translationGroup->addChild(_originFixGroup.get());
- _translationGroup->setMatrix(osg::Matrix::translate(point));
- _origin = point;
- _normal = normal;
- osg::Vec3 shift = normal.operator*(10.0f);
- _trackOrigin = shift.operator+(point);
- printf("TrackPoint is at %lf %lf %lf\n", _trackOrigin.x(), _trackOrigin.y(), _trackOrigin.z());
- }
- osg::ref_ptr<osg::MatrixTransform> TrackPoint::getUppermostRoot() {
- return _translationGroup.get();
- }
- osg::Vec3 TrackPoint::getTranslation() {
- return _origin;
- }
- osg::Vec3 TrackPoint::getRotation() {
- printf("YNorm: %lf %lf %lf\n", _normal.x(), _normal.y(), _normal.z());
- osg::Vec3 start = osg::Vec3(0.0f, 0.0f, 1.0f);
- // From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
- osg::Quat quat = osg::Quat(0.0f, 0.0f, 0.0f, 0.0f);
- quat.makeRotate(start, _normal);
- float sinr_cosp = 2 * (quat.w() * quat.x() + quat.y() * quat.z());
- float cosr_cosp = 1 - 2 * (quat.x() * quat.x() + quat.y() * quat.y());
- float xRotation = std::atan2(sinr_cosp, cosr_cosp) * 180.0 / M_PI;
- float sinp = 2 * (quat.w() * quat.y() - quat.z() * quat.x());
- float yRotation;
- if (std::abs(sinp) >= 1) {
- yRotation = std::copysign(M_PI / 2, sinp) * 180.0 / M_PI;
- } else {
- yRotation = std::asin(sinp) * 180.0 / M_PI;
- }
- float siny_cosp = 2 * (quat.w() * quat.z() + quat.x() * quat.y());
- float cosy_cosp = 1 - 2 * (quat.y() * quat.y() + quat.z() * quat.z());
- float zRotation = std::atan2(siny_cosp, cosy_cosp) * 180.0 / M_PI;
- return osg::Vec3(xRotation, yRotation, zRotation);
- }
- osg::Vec3 TrackPoint::getTrackPoint() {
- return _trackOrigin;
- }
|