OSG_BASE_DLLMAPPING bool MatrixLookAt(OSG::Matrix &result,
OSG::Pnt3f from,
OSG::Pnt3f at,
OSG::Vec3f up )
{
Vec3f view;
Vec3f right;
Vec3f newup;
Vec3f tmp;
view = from - at;
view.normalize();
right = up.cross(view);
if(right.dot(right) < TypeTraits<Real>::getDefaultEps())
{
return true;
}
right.normalize();
newup = view.cross(right);
result.setIdentity ();
result.setTranslate(from[0], from[1], from[2]);
Matrix tmpm;
tmpm.setValue(right, newup, view);
result.mult(tmpm);
return false;
}
btCollisionShape* VRPhysics::getConvexShape() {
btConvexHullShape* shape = new btConvexHullShape();
OSG::Matrix m;
OSG::Matrix M = vr_obj->getWorldMatrix();
M.invert();
vector<OSG::VRObject*> geos = vr_obj->getObjectListByType("Geometry");
for (auto g : geos) {
OSG::VRGeometry* geo = (OSG::VRGeometry*)g;
if (geo == 0) continue;
if (geo->getMesh() == 0) continue;
OSG::GeoVectorPropertyRecPtr pos = geo->getMesh()->getPositions();
if (pos == 0) continue;
if (geo != vr_obj) {
m = geo->getWorldMatrix();
m.multLeft(M);
}
for (unsigned int i = 0; i<pos->size(); i++) {
OSG::Pnt3f p;
pos->getValue(p,i);
if (geo != vr_obj) m.mult(p,p);
for (int i=0; i<3; i++) p[i] *= scale[i];
shape->addPoint(btVector3(p[0], p[1], p[2]));
}
}
shape->setMargin(collisionMargin);
//cout << "\nConstruct Convex shape for " << vr_obj->getName() << endl;
return shape;
}
OSG::Matrix VRPhysics::fromBTTransform(const btTransform t, OSG::Vec3f scale) {
OSG::Matrix m = fromBTTransform(t);
// apply scale
OSG::Matrix s;
s.setScale(scale);
m.mult(s);
return m;
}
OSG_BASE_DLLMAPPING bool MatrixLookAt(OSG::Matrix &result,
OSG::Real32 fromx,
OSG::Real32 fromy,
OSG::Real32 fromz,
OSG::Real32 atx,
OSG::Real32 aty,
OSG::Real32 atz,
OSG::Real32 upx,
OSG::Real32 upy,
OSG::Real32 upz)
{
Vec3f view;
Vec3f right;
Vec3f newup;
Vec3f up;
view.setValues(fromx - atx , fromy - aty, fromz - atz);
view.normalize();
up.setValues(upx, upy, upz);
right = up.cross(view);
if(right.dot(right) < TypeTraits<Real>::getDefaultEps())
{
return true;
}
right.normalize();
newup = view.cross(right);
result.setIdentity ();
result.setTranslate(fromx, fromy, fromz);
Matrix tmpm;
tmpm.setValue(right, newup, view);
result.mult(tmpm);
return false;
}
/*! The mouseMove is called by the viewer when the mouse is moved in the
viewer and this handle is the active one.
\param x the x-pos of the mouse (pixel)
\param y the y-pos of the mouse (pixel)
*/
void PlaneMoveManipulator::mouseMove(const Int16 x,
const Int16 y)
{
SLOG << "==============================" << endLog;
SLOG << "PlaneMoveManipulator::mouseMove() enter x=" << x << " y=" << y << endLog;
// get the beacon's core (must be ComponentTransform) and it's center
if( getTarget() == NULL )
{
SWARNING << "Handle has no target.\n";
return;
}
// get transformation of beacon
Transform *t = dynamic_cast<Transform *>(getTarget()->getCore());
if( t == NULL )
{
SWARNING << "handled object has no parent transform!\n";
return;
}
Vec3f translation; // for matrix decomposition
Quaternion rotation;
Vec3f scaleFactor;
Quaternion scaleOrientation;
t->getMatrix().getTransform(translation, rotation, scaleFactor,
scaleOrientation);
OSG::Line viewray;
getViewport()->getCamera()->calcViewRay(viewray, x, y, *getViewport());
SLOG << "Manipulator::mouseMove(): viewray: " << viewray << endLog;
// Get manipulator axes into world space
OSG::Matrix tm = getTarget()->getToWorld();
Vec3f rot_axis;
tm.multFull(Vec3f(0,1,0), rot_axis);
Plane pl(rot_axis, getClickPoint());
Pnt3f plpoint;
if (pl.intersect(viewray, plpoint) == true) // Ignore moving out of the plane...
{
SLOG << "Manipulator::mouseMove(): plpoint: " << plpoint << endLog;
Vec3f trans = getBaseTranslation();
Quaternion rot = getBaseRotation();
// Get manipulator axes into world space
Vec3f xp,zp;
tm.multFull(Vec3f(1,0,0), xp);
tm.multFull(Vec3f(0,0,1), zp);
if (getActiveSubHandle() == getHandleXNode())
{
Line xaxis(getClickPoint(), xp);
Line zaxis(getClickPoint(), zp);
Real32 fx = xaxis.getClosestPointT(plpoint);
Real32 fz = zaxis.getClosestPointT(plpoint);
SLOG << "Manipulator::mouseMove(): xaxis: " << xaxis << " zaxis: " << zaxis <<endLog;
SLOG << "Manipulator::mouseMove(): fx: " << fx << " fz: " << fz <<endLog;
// Alternative: transform hitpoint into manip space
OSG::Matrix m = getTarget()->getToWorld();
m.invert();
Pnt3f mpoint;
m.mult(plpoint, mpoint);
SLOG << "Manipulator::mouseMove(): mpoint:" << mpoint << endLog;
trans = trans + xp * fx + zp * fz;
}
else if (getActiveSubHandle() == getHandleZNode())
{
Pnt3f wcenter;
tm.multFull(Pnt3f(0,getLength()[1],0), wcenter);
Vec3f vclick, vcurrent;
vclick = getClickPoint() - wcenter;
vcurrent = plpoint - wcenter;
vclick.normalize();
vcurrent.normalize();
Real32 a = vclick.enclosedAngle(vcurrent);
SLOG << "Manipulator::mouseMove(): wcenter:" << wcenter << "" <<endLog;
SLOG << "Manipulator::mouseMove(): vclick:" << vclick << " vcurrent:" << vcurrent <<endLog;
SLOG << "Manipulator::mouseMove(): angle:" << a << " deg: " << osgRad2Degree(a) << endLog;
}
Matrix m;
m.setTransform(trans, rot, scaleFactor, scaleOrientation);
t->setMatrix(m);
}
setLastMousePos(Pnt2f(Real32(x), Real32(y)));
updateHandleTransform();
//SLOG << "Manipulator::mouseMove() leave\n" << std::flush;
}