NewActionTypes::ResultE Joint::intersectActorEnter(
ActorBase::FunctorArgumentType &funcArg)
{
IntersectActor *pIA = dynamic_cast<IntersectActor *>(
funcArg.getActor());
Matrix matrix = this->getMatrix();
Line transLine;
Pnt3f pos;
Vec3f dir;
matrix.mult(this->getJointTransformation());
matrix.invert();
matrix.multFull(pIA->getRay().getPosition (), pos);
matrix.mult (pIA->getRay().getDirection(), dir);
transLine.setValue(pos, dir);
pIA->beginEditState();
{
pIA->setRay (transLine );
pIA->setScaleFactor(pIA->getScaleFactor() / dir.length());
}
pIA->endEditState ();
pIA->setupChildrenPriorities();
return NewActionTypes::Continue;
}
ActionBase::ResultE Joint::intersectEnter(Action *action)
{
// Use parent class for trivial reject
if(Inherited::intersect(action) == Action::Skip)
return Action::Skip;
// Need to check children
IntersectAction *ia = dynamic_cast<IntersectAction *>(action);
Matrix m = this->getMatrix();
m.mult(this->getJointTransformation());
m.invert();
Pnt3f pos;
Vec3f dir;
m.multFull(ia->getLine().getPosition (), pos);
m.mult (ia->getLine().getDirection(), dir);
Real32 length = dir.length();
if(length < TypeTraits<Real32>::getDefaultEps())
SWARNING << "Joint::intersectEnter: Near-zero scale!" << std::endl;
ia->setLine(Line(pos, dir), ia->getMaxDist());
ia->scale (length );
return ActionBase::Continue;
}
bool VRMouse::calcViewRay(PerspectiveCameraRecPtr pcam, Line &line, float x, float y, int W, int H){
if(W <= 0 || H <= 0) return false;
Matrix proj, projtrans, view;
pcam->getProjection(proj, W, H);
pcam->getProjectionTranslation(projtrans, W, H);
Matrix wctocc;
wctocc.mult(proj);
wctocc.mult(projtrans);
Matrix cctowc;
cctowc.invertFrom(wctocc);
Pnt3f from, at;
multFull(cctowc, Pnt3f(x, y, -1), from);
multFull(cctowc, Pnt3f(x, y, 0.1), at );
Vec3f dir = at - from;
line.setValue(from, dir);
return true;
}
bool Camera::calcViewRay( Line &line,
Int32 x,
Int32 y,
const Viewport &port,
Real32 *t )
{
if(port.getPixelWidth() <= 0 || port.getPixelHeight() <= 0)
{
return false;
}
Matrix proj, projtrans, view;
getProjection(proj,
port.getPixelWidth(),
port.getPixelHeight());
getProjectionTranslation(projtrans,
port.getPixelWidth(),
port.getPixelHeight());
getViewing(view,
port.getPixelWidth(),
port.getPixelHeight());
Matrix wctocc = proj;
wctocc.mult(projtrans);
wctocc.mult(view);
Matrix cctowc;
cctowc.invertFrom(wctocc);
Real32 rx(0.f), ry(0.f);
port.getNormalizedCoordinates(rx, ry, x, y);
Pnt3f from, at;
cctowc.multFull(Pnt3f(rx, ry, -1), from);
cctowc.multFull(Pnt3f(rx, ry, 1), at );
Vec3f dir = at - from;
if(t != NULL)
{
*t = dir.length();
}
line.setValue(from, dir);
return true;
}
void VRSnappingEngine::update() {
for (auto dev : VRSetupManager::getCurrent()->getDevices()) { // get dragged objects
VRTransformPtr obj = dev.second->getDraggedObject();
VRTransformPtr gobj = dev.second->getDraggedGhost();
if (obj == 0 || gobj == 0) continue;
if (objects.count(obj) == 0) continue;
Matrix m = gobj->getWorldMatrix();
Vec3f p = Vec3f(m[3]);
bool lastEvent = event->snap;
event->snap = 0;
for (auto ri : rules) {
Rule* r = ri.second;
if (r->csys == obj) continue;
if (anchors.count(obj)) {
for (auto a : anchors[obj]) {
Matrix maL = a->getMatrix();
Matrix maW = m; maW.mult(maL);
Vec3f pa = Vec3f(maW[3]);
Vec3f paL = r->local( Vec3f(maW[3]) );
Vec3f psnap = r->getSnapPoint(pa);
float D = (psnap-paL).length(); // check distance
//cout << "dist " << D << " " << pa[1] << " " << paL[1] << " " << psnap[1] << endl;
if (!r->inRange(D)) continue;
r->snap(m);
maL.invert();
m.mult(maL);
event->set(obj, r->csys, m, dev.second, 1);
break;
}
} else {
Vec3f p2 = r->getSnapPoint(p);
float D = (p2-p).length(); // check distance
if (!r->inRange(D)) continue;
r->snap(m);
event->set(obj, r->csys, m, dev.second, 1);
}
}
obj->setWorldMatrix(m);
if (lastEvent != event->snap) {
if (event->snap) snapSignal->trigger<EventSnap>(event);
else if (obj == event->o1) snapSignal->trigger<EventSnap>(event);
}
}
// update geo
if (!hintGeo->isVisible()) return;
}
Real32 DistanceLOD::calcDistance(DrawActionBase *pAction,
const Matrix &mToWorld)
{
Matrix m;
m.invertFrom(mToWorld);
m.mult(pAction->getCameraToWorld());
Pnt3f eyepos;
m.mult(eyepos, eyepos);
_lastDist = eyepos.dist(getCenter());
return _lastDist;
}
Vec2f VRIntersect_computeTexel(VRIntersection& ins, NodeRecPtr node) {
if (!ins.hit) return Vec2f(0,0);
if (node == 0) return Vec2f(0,0);
GeometryRefPtr geo = dynamic_cast<Geometry*>( node->getCore() );
if (geo == 0) return Vec2f(0,0);
auto texcoords = geo->getTexCoords();
if (texcoords == 0) return Vec2f(0,0);
TriangleIterator iter = geo->beginTriangles(); iter.seek( ins.triangle );
Matrix m = node->getToWorld();
m.invert();
Pnt3f local_pnt; m.mult(ins.point, local_pnt);
Pnt3f p0 = iter.getPosition(0);
Pnt3f p1 = iter.getPosition(1);
Pnt3f p2 = iter.getPosition(2);
Vec3f cr = (p1 - p0).cross(p2 - p0);
Vec3f n = cr; n.normalize();
float areaABC = n.dot(cr);
float areaPBC = n.dot((p1 - local_pnt).cross(p2 - local_pnt));
float areaPCA = n.dot((p2 - local_pnt).cross(p0 - local_pnt));
float a = areaPBC / areaABC;
float b = areaPCA / areaABC;
float c = 1.0f - a - b;
return iter.getTexCoords(0) * a + iter.getTexCoords(1) * b + iter.getTexCoords(2) * c;
}
void Particles::setScale(gmtl::Vec3f scale)
{
float maxScale;
Vec3f position, osgScale;
Quaternion orientation, scaleOrientation;
beginEditCP(particleTrans);
Matrix mOrig = particleTrans->getMatrix();
endEditCP(particleTrans);
Matrix mNew = mOrig;
mNew.getTransform(position, orientation, osgScale, scaleOrientation);
osgScale = Vec3f(scale[0], scale[1], scale[2]);
mNew.setTransform(position, orientation, osgScale, scaleOrientation);
beginEditCP(particleTrans, Transform::MatrixFieldMask);
particleTrans->setMatrix(mNew);
endEditCP(particleTrans, Transform::MatrixFieldMask);
mOrig.invert();
// mNew.multLeft(mOrig);
mNew.mult(mOrig);
beginEditCP(particleTransNode);
DynamicVolume &v = particleTransNode->getVolume(false);
v.transform(mNew);
endEditCP(particleTransNode);
} // setScale
Matrix PhysicsGeom::getTransformation(void) const
{
Matrix Transformation;
Vec3f Translation;
const dReal* t = dGeomGetOffsetPosition(_GeomID);
Translation.setValues( t[0],t[1],t[2] );
dQuaternion q;
dGeomGetOffsetQuaternion(_GeomID, q);
Quaternion Rotation;
Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);
Transformation.setTransform(Translation,Rotation);
if(isPlaceable())
{
t = dGeomGetPosition(_GeomID);
Translation.setValues( t[0],t[1],t[2] );
dGeomGetQuaternion(_GeomID, q);
Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);
Matrix NonBodyTransformation;
NonBodyTransformation.setTransform(Translation,Rotation);
Transformation.mult(NonBodyTransformation);
}
return Transformation;
}
Vector3 Polygon::MultiplyByMatrix(Vector3 v, Matrix<4,4> m)
{
Matrix<4, 1> a = {v.x, v.y, v.z, 1};
Matrix<4, 1> r = {0, 0, 0, 0};
a.mult(m, a, r);
return Vector3(r.m[0][0], r.m[1][0], r.m[2][0]);
}
void SoundEmitter::update(EventDetails* const details)
{
OSG_ASSERT(getParents().size() == 1 && "A Sound Emitter NodeCore MUST have 1 and only 1 parent.");
Matrix wm;
dynamic_cast<Node*>(_mfParents[0])->getToWorld(wm);
Pnt3f Position(0, 0, 0);
wm.mult(Pnt3f(0.0f,0.0f,0.0f),Position);
if(getSound() != NULL)
{
//Remove all invalid
for(std::set<UInt32>::iterator Itor(_EmittedSoundChannels.begin()) ; Itor != _EmittedSoundChannels.end() ;)
{
if(!getSound()->isValid(*Itor))
{
std::set<UInt32>::iterator EraseItor(Itor);
++Itor;
_EmittedSoundChannels.erase(EraseItor);
}
else
{
//getSound()->setChannelVelocity(_PreviousPosition - Position * (1.0f/dynamic_cast<UpdateEventDetails* const>(details)->getElapsedTime()), *Itor);
getSound()->setChannelPosition(Position, *Itor);
++Itor;
}
}
}
_PreviousPosition = Position;
}
ActionBase::ResultE Joint::intersectLeave(Action *action)
{
IntersectAction *ia = dynamic_cast<IntersectAction *>(action);
Matrix m = this->getMatrix();
m.mult(this->getJointTransformation());
Pnt3f pos;
Vec3f dir;
m.multFull(ia->getLine().getPosition (), pos);
m.mult (ia->getLine().getDirection(), dir);
ia->setLine(Line(pos, dir), ia->getMaxDist());
ia->scale(dir.length());
return ActionBase::Continue;
}
void RenderPartition::calcFrustum(void)
{
Matrix pr = _oDrawEnv.getCameraFullProjection();
pr.mult(_oDrawEnv.getCameraViewing());
_oFrustum.setPlanes(pr);
}
void ShaderShadowMapEngine::calcPointLightMatrices(
Matrix &matWorldToLight,
Matrix &matEyeToLight,
const PointLight *pointL,
const Matrix &matEyeToWorld)
{
if(pointL->getBeacon() != NULL)
pointL->getBeacon()->getToWorld(matWorldToLight);
Matrix matLightPos;
matLightPos .setTranslate(pointL->getPosition());
matWorldToLight.mult (matLightPos );
matWorldToLight.invert ( );
matEyeToLight = matWorldToLight;
matEyeToLight.mult(matEyeToWorld);
}
SystemState SystemState::operator*(double val) const
{
SystemState res = *this;
res.speed *= val;
Matrix mt = res.pos.toVector();
mt.mult(val);
res.pos = Position(mt);
return res;
}
vector<Vec3f> polygon::toSpace(Matrix m) {
vector<Vec3f> res;
for (auto p : points) {
Vec3f pp = Vec3f(p[0], p[1], -sqrt(1-(p[0]*p[0]+p[1]*p[1])));
m.mult(pp,pp);
res.push_back(pp);
}
return res;
}
Vec3f local(Vec3f p) {
if (csys) {
C = csys->getWorldMatrix();
C.invert();
Pnt3f pL;
C.mult(p,pL);
return Vec3f(pL);
} else return p;
}
//multiplizirt alle matrizen in dem vector zusammen
Matrix VRTransform::computeMatrixVector(vector<Matrix> tv) {
if (tv.size() == 0) return Matrix();
Matrix m;
for (unsigned int i=0; i<tv.size(); i++) {
m.mult(tv[tv.size()-i-1]);
}
return m;
}
/*! Calculates \a matWorldToLight and \a matEyeToLight for a directional light
\a dirL and inverse viewing matrix \a matEyeToWorld.
*/
void ShaderShadowMapEngine::calcDirectionalLightMatrices(
Matrix &matWorldToLight,
Matrix &matEyeToLight,
const DirectionalLight *dirL,
const Matrix &matEyeToWorld)
{
if(dirL->getBeacon() != NULL)
dirL->getBeacon()->getToWorld(matWorldToLight);
Quaternion rotLightDir (Vec3f(0.f, 0.f, 1.f), dirL->getDirection());
Matrix matLightDir;
matLightDir.setRotate(rotLightDir);
matWorldToLight.mult (matLightDir);
matWorldToLight.invert( );
matEyeToLight = matWorldToLight;
matEyeToLight.mult(matEyeToWorld);
}
void
updateRayGeo(void)
{
Line &ray = testRays[uiCurrentRay];
IntersectResult &res = resultsP[uiCurrentRay];
Pnt3f startPnt = ray.getPosition();
Pnt3f endPnt = startPnt + (rayLength * ray.getDirection());
beginEditCP(pPoints);
pPoints->setValue(startPnt, 0);
pPoints->setValue(endPnt, 1);
if(res._hit == true)
{
TriangleIterator triIt(res._pObj);
Matrix matrix;
Pnt3f point;
triIt.seek(res._tri);
res._pObj->getToWorld(matrix);
point = triIt.getPosition(0);
matrix.mult(point, point);
pPoints->setValue(point, 2);
point = triIt.getPosition(1);
matrix.mult(point, point);
pPoints->setValue(point, 3);
point = triIt.getPosition(2);
matrix.mult(point, point);
pPoints->setValue(point, 4);
}
else
{
pPoints->setValue(Pnt3f(0.0, 0.0, 0.0), 2);
pPoints->setValue(Pnt3f(0.0, 0.0, 0.0), 3);
pPoints->setValue(Pnt3f(0.0, 0.0, 0.0), 4);
}
endEditCP (pPoints);
}
void Camera::getWorldToScreen(Matrix &result, const Viewport& p)
{
Matrix mv,prt,pr;
getProjection (result, p.getPixelWidth(), p.getPixelHeight());
getProjectionTranslation(prt , p.getPixelWidth(), p.getPixelHeight());
getViewing (mv , p.getPixelWidth(), p.getPixelHeight());
result.mult(prt);
result.mult(mv );
}
/** Cast a ray in world coordinates from the object in its local coordinates, -z axis defaults **/
Line VRTransform::castRay(Vec3f dir) {
Matrix m;
getWorldMatrix(m);
m.mult(dir,dir); dir.normalize();
Pnt3f p0 = Vec3f(m[3]);// + dir*0.5;
Line ray;
ray.setValue(p0, dir);
return ray;
}
void VRTransform::computeMatrix() {
Matrix mm;
MatrixLookAt(mm, _from, _at, _up);
if (_scale != Vec3f(1,1,1)) {
Matrix ms;
ms.setScale(_scale);
mm.mult(ms);
}
dm->write(mm);
}
Pnt2f Manipulator::calcScreenProjection(const Pnt3f & p,
Viewport * const port)
{
Camera *cam;
Matrix proj, projtrans, view;
Pnt3f pnt;
if( port != NULL )
{
cam = port->getCamera();
cam->getProjection(proj,
port->calcPixelWidth(),
port->calcPixelHeight());
cam->getProjectionTranslation(projtrans,
port->calcPixelWidth(),
port->calcPixelHeight());
cam->getViewing(view,
port->calcPixelWidth(),
port->calcPixelHeight());
Matrix wctocc = proj;
wctocc.mult(projtrans);
wctocc.mult(view);
wctocc.multFull(p, pnt);
Real32 rx = (pnt[0] + 1.0) /2 * port->calcPixelWidth();
Real32 ry = (pnt[1] + 1.0) /2 * port->calcPixelHeight();
return Pnt2f(rx, ry);
}
else
{
SWARNING << "calcScreenProjection(const Pnt3f&, "
"Viewport * const port="
<< port << ")\n";
return Pnt2f(0.0f, 0.0f);
}
}
/*! Calculates \a matWorldToLight and \a matEyeToLight for a spot light
\a spotL and inverse viewing matrix \a matEyeToWorld.
*/
void ShaderShadowMapEngine::calcSpotLightMatrices(
Matrix &matWorldToLight,
Matrix &matEyeToLight,
const SpotLight *spotL,
const Matrix &matEyeToWorld)
{
if(spotL->getBeacon() != NULL)
spotL->getBeacon()->getToWorld(matWorldToLight);
Matrix matLightPos;
matLightPos.setTranslate(spotL->getPosition());
Matrix matLightDir;
Quaternion rotLightDir(Vec3f(0.f, 0.f, 1.f), -spotL->getDirection());
matLightDir.setRotate(rotLightDir);
matWorldToLight.mult (matLightPos);
matWorldToLight.mult (matLightDir);
matWorldToLight.invert( );
matEyeToLight = matWorldToLight;
matEyeToLight.mult(matEyeToWorld);
}
//read matrix from doublebuffer and apply it to transformation
//should be called from the main thread only
void VRTransform::updatePhysics() {
//update bullets transform
if (noBlt and !held) { noBlt = false; return; }
if (!physics->isPhysicalized()) return;
Matrix m;
dm->read(m);
Matrix pm;
getWorldMatrix(pm, true);
pm.mult(m);
physics->updateTransformation(pm);
physics->pause();
physics->resetForces();
}
void MatrixCameraDecorator::getViewing(Matrix &result,
UInt32 width, UInt32 height)
{
CameraPtr camera = getDecoratee();
if(camera == NullFC)
{
FWARNING(("MatrixCameraDecorator::getViewing: no decoratee!\n"));
result.setIdentity();
return;
}
camera->getViewing(result, width, height);
result.multLeft(getPostViewing());
result.mult(getPreViewing());
}
void MatrixCameraDecorator::getProjectionTranslation(Matrix &result,
UInt32 width, UInt32 height)
{
CameraPtr camera = getDecoratee();
if(camera == NullFC)
{
FWARNING(("MatrixCameraDecorator::getProjectionTranslation: no decoratee!\n"));
result.setIdentity();
return;
}
camera->getProjectionTranslation(result, width, height);
result.multLeft(getPostProjectionTranslation());
result.mult(getPreProjectionTranslation());
}
Action::ResultE transformEnter(CNodePtr& node, Action * action)
{
IntersectAction * ia = dynamic_cast<IntersectAction*>(action);
NodePtr n( node );
Transform* core = dynamic_cast<Transform*>(get_pointer(n->getCore()));
Matrix m = core->getMatrix();
m.invert();
Pnt3f pos;
Vec3f dir;
m.multFull(ia->getLine().getPosition (), pos);
m.mult (ia->getLine().getDirection(), dir);
ia->setLine( Line( pos, dir ), ia->getMaxDist() );
return Action::Continue;
}
void DSLightChunk::calcModelView(DrawEnv *drawEnv, Matrix &result)
{
result = drawEnv->getVPCameraViewing();
Matrix toWorld;
if(_sfBeacon.getValue() != NULL)
{
_sfBeacon.getValue()->getToWorld(toWorld);
}
else
{
toWorld.setIdentity();
}
result.mult(toWorld);
}
