/*
 * Scene3DView.cpp
 *
 * Copyright (C) 1999 Stephen F. White
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see the file "COPYING" for details); if 
 * not, write to the Free Software Foundation, Inc., 675 Mass Ave, 
 * Cambridge, MA 02139, USA.
 */

#include "stdafx.h"

#include "swt.h"

#include "Scene3DView.h"
#include "Scene.h"
#include "Path.h"
#include "Node.h"
#include "NodeTransform.h"
#include "Matrix.h"
#include "SFRotation.h"
#include "SFVec3f.h"
#include "EulerAngles.h"
#include "NodeViewpoint.h"
#include "InputDeviceTime.h"
#include "NodeNurbsSurface.h"
#include "NodeNurbsGroup.h"
#include "Util.h"

static int 
InputDeviceTimerCallback(void *data)
{
    return((Scene3DView *) data)->readInputDevice();
}

Scene3DView::Scene3DView(Scene *scene, SWND parent)
  : SceneView(scene, parent)
{
    _dc = NULL;
    _glc = NULL;
    _trackMouse = false;
    _cursorArrow = swLoadCursor(SW_CURSOR_ARROW);
    _cursorZoom = swLoadCursor(SW_CURSOR_DBL_ARROW_VERT);
    _cursorRotate = swLoadCursor(SW_CURSOR_CIRCLE);
    _cursorMove = swLoadCursor(SW_CURSOR_CROSS);
    _cursorNone = swLoadCursor(SW_CURSOR_NONE);
    _button1down = false;
    _button2down = false;
    _fieldEdit = false;
    _InputDeviceTimer = NULL;
    if (TheApp->getNumberInputDevices() != 0)
      _InputDeviceTimer = swSetTimer(_wnd, INPUTDEVICE_TIME, 
                                     InputDeviceTimerCallback, this);
    ProtoTransform *transProto = (ProtoTransform *)scene->getProto("Transform");
    _transCenterField      = transProto->center;
    _transRotationField    = transProto->rotation;
    _transScaleField       = transProto->scale;
    _transTranslationField = transProto->translation;

    ProtoViewpoint *viewProto = (ProtoViewpoint *)scene->getProto("Viewpoint");
    _viewOrientationField  = viewProto->orientation;
    _viewPositionField     = viewProto->position;
    _mouseX = INT_MIN;
    _mouseY = INT_MIN;
    _hitHandle = false;
    _offset.x = 0;
    _offset.y = 0;
    _offset.z = 0;
}

Scene3DView::~Scene3DView()
{
    if (TheApp->getNumberInputDevices() != 0)
        if (_InputDeviceTimer != NULL)
            swKillTimer(_InputDeviceTimer);
    if (_glc != NULL)
        swDestroyGLContext(_glc);
    _glc = NULL;
    swDestroyDC(_dc);
    // early/double destroy of window is protected by destroySelf()
    swDestroyWindow(_wnd);

    swDestroyCursor(_cursorArrow);
    swDestroyCursor(_cursorZoom);
    swDestroyCursor(_cursorRotate);
    swDestroyCursor(_cursorMove);
//    swDestroyCursor(_cursorNone);
}

void Scene3DView::OnUpdate(SceneView *sender, int type, Hint *hint)
{
    // inputdevice reading routine need information if about field editing
    if (type==UPDATE_START_FIELD_EDIT)
       _fieldEdit = true;
    else if ((type==UPDATE_STOP_FIELD_EDIT) && (_fieldEdit))
       {
       _fieldEdit = false;
       // restart inputdevice reading timer 
       if (TheApp->getNumberInputDevices() != 0)
          _InputDeviceTimer = swSetTimer(_wnd, INPUTDEVICE_TIME,
                                         InputDeviceTimerCallback, this);
       }
    else if (type == UPDATE_FIELD)
       {
       NodeUpdate *nodeUpdate = (NodeUpdate *) hint;
       if (nodeUpdate->node != NULL)
          nodeUpdate->node->update();
       }

    swInvalidateWindow(_wnd);
}

void Scene3DView::OnDraw(int /* x */, int /* y */,
			 int /* width */, int /* height */) 
{
    int		width, height;
    if (!_dc) _dc = swCreateDC(_wnd);
    if (!_glc) _glc = swCreateGLContext(_dc);
    swMakeCurrent(_dc, _glc);
    swGetSize(_wnd, &width, &height);
    glViewport(0, 0, width, height);
    TheApp->setEyeMode(EM_NONE);
    if (TheApp->isAnaglyphStereo()) {
        glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
        glClearAccum(0.0,0.0,0.0,0.0); 
        glClearColor(0.0,0.0,0.0,0.0);
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    }
    if ((TheApp->canQuadBufferStereo()) || (TheApp->useStereo())) {
       if (TheApp->useStereo())
           TheApp->setEyeMode(EM_LEFT);
       if (TheApp->canQuadBufferStereo())
            glDrawBuffer(GL_BACK_LEFT);
       if (TheApp->isAnaglyphStereo()) {
           glClear(GL_ACCUM_BUFFER_BIT);
           if (TheApp->getStereoType() == RED_GREEN_ANAGLYPH_STEREO)
               glColorMask(GL_FALSE, GL_TRUE, GL_FALSE, GL_TRUE);
           else if (TheApp->getStereoType() == RED_BLUE_ANAGLYPH_STEREO)
               glColorMask(GL_FALSE, GL_FALSE, GL_TRUE, GL_TRUE);
           else 
               glColorMask(GL_TRUE, GL_FALSE, GL_FALSE, GL_TRUE);
       }
       _scene->drawScene(false,0,0);
       if (TheApp->useStereo())
           if ((_mouseX != INT_MIN) && (_mouseY != INT_MIN))
               _scene->draw3dCursor(_mouseX, _mouseY);
       if (TheApp->isAnaglyphStereo())
           glAccum(GL_ACCUM, 1.0f);

       if (TheApp->useStereo())
           TheApp->setEyeMode(EM_RIGHT);

       if (TheApp->canQuadBufferStereo())
           glDrawBuffer(GL_BACK_RIGHT);
       if (TheApp->isAnaglyphStereo()) {
           glClear(GL_ACCUM_BUFFER_BIT);
           if (TheApp->getStereoType() == GREEN_RED_ANAGLYPH_STEREO)
               glColorMask(GL_FALSE, GL_TRUE, GL_FALSE, GL_TRUE);
           else if (TheApp->getStereoType() == BLUE_RED_ANAGLYPH_STEREO)
               glColorMask(GL_FALSE, GL_FALSE, GL_TRUE, GL_TRUE);
           else
               glColorMask(GL_TRUE, GL_FALSE, GL_FALSE, GL_TRUE);        
       } 
       _scene->drawScene(false,0,0);
       if (TheApp->useStereo())
           if ((_mouseX != INT_MIN) && (_mouseY != INT_MIN))
               _scene->draw3dCursor(_mouseX, _mouseY);
       if (TheApp->isAnaglyphStereo())
           glAccum(GL_ACCUM, 1.0f);
       }
    else
       {
       _scene->drawScene(false,0,0);
       }
    if (TheApp->isAnaglyphStereo())
        glAccum(GL_RETURN,1.0);
    swSwapBuffers(_dc, _glc);
}

void Scene3DView::OnSize(int width, int height) 
{
    swInvalidateWindow(_wnd);
}

void Scene3DView::OnKeyDown(int key, int x, int y, int modifiers) 
{
    switch (key) {
      case SW_UP:
	_scene->moveCamera(0.0f, 0.0f, -1.0f);
	swInvalidateWindow(_wnd);
	break;
      case SW_DOWN:
	_scene->moveCamera(0.0f, 0.0f, 1.0f);
	swInvalidateWindow(_wnd);
	break;
      case SW_KEY_LEFT:
	_scene->turnCamera(0.0f, 1.0f, 0.0f, 0.10f);
	swInvalidateWindow(_wnd);
	break;
      case SW_KEY_RIGHT:
	_scene->turnCamera(0.0f, 1.0f, 0.0f, -0.10f);
	swInvalidateWindow(_wnd);
	break;
      default:
	SceneView::OnKeyDown(key, x, y, modifiers);
	break;
    }
}

void Scene3DView::OnMButtonDown(int x, int y, int modifiers) 
{
    _backedUp = false;
    int		width, height;
    swGetSize(_wnd, &width, &height);

    swMakeCurrent(_dc, _glc);
    swSetFocus(_wnd);
    if (modifiers & SW_ALT) 
       if (_button1down)
          swSetCursor(_wnd, _cursorZoom);
       else 
          swSetCursor(_wnd, _cursorMove);
    else if ( _scene->getNavigationMode()) 
       if (_button1down)
          swSetCursor(_wnd, _cursorMove);
       else 
          swSetCursor(_wnd, _cursorZoom);
    _trackMouse = true;
    swSetCapture(_wnd);
    _trackX = x;
    _trackY = y;

    _button2down = true;
}

void Scene3DView::OnRButtonDown(int x, int y, int modifiers) 
{
    // to be filled later...
}

void Scene3DView::OnLButtonDown(int x, int y, int modifiers) 
{
    Path   *path;
    _backedUp = false;
    int		width, height;

    swGetSize(_wnd, &width, &height);

    swMakeCurrent(_dc, _glc);
    swSetFocus(_wnd);

    _hitHandle = false;
    bool cursorChanged = false;

    if (modifiers & SW_CONTROL)  {
        cursorChanged = true;
        if (modifiers & SW_SHIFT) 
	    swSetCursor(_wnd, _cursorMove);
        else
	    swSetCursor(_wnd, _cursorRotate);
    } else if (modifiers & SW_ALT) {
        cursorChanged = true;
	if (_button2down)
           swSetCursor(_wnd, _cursorZoom);
        else
           swSetCursor(_wnd, _cursorRotate);
    } else if (modifiers & SW_SHIFT) {
        cursorChanged = true;
	swSetCursor(_wnd, _cursorZoom);
    } else {
      
	path = _scene->pick(x, height - y);
        if (path) {

	    _scene->setSelection(path);
	    _scene->UpdateViews(NULL, UPDATE_SELECTION);
	    Node   *node = _scene->getSelection()->getNode();
	    int	   handle = _scene->getSelectedHandle();

	    if (handle != -1) {
 
                _hitHandle = true;		

		int	    constraint = CONSTRAIN_NONE, field;
		Vec3f	v = node->getHandle(handle, &constraint, &field);

		_backedUp = false;

		// now calculate an offset in object space from the mouse 
		// to the actual handle position

		glPushMatrix();
		Vec3f	    o;
		float	    px, py, pz;
		_scene->transform(_scene->getSelection());
		node->transformForHandle(handle);
		_scene->projectPoint(v.x, v.y, v.z, &px, &py, &pz);
		_scene->unProjectPoint((float) x, (float) height - (float) y, pz,
				       &o.x, &o.y, &o.z);
		glPopMatrix();

		_offset = v - o;
	    } 
	} 
    }
    if ((!_hitHandle) && (_scene->getNavigationMode())) {
        if (!cursorChanged)
	    if (_button2down)
                swSetCursor(_wnd, _cursorMove);
            else
                swSetCursor(_wnd, _cursorRotate);
    }
    _trackMouse = true;
    swSetCapture(_wnd);
    _trackX = x;
    _trackY = y;
    _button1down = true;
}

void Scene3DView::OnMouseEnter()
{
    _scene->setHasFocus();
    _scene->use3dCursor(true);
    if (TheApp->useStereo())
        swSetCursor(_wnd, _cursorNone);
    swInvalidateWindow(_wnd);
} 

void Scene3DView::OnMouseLeave()
{
    _scene->deleteHasFocus();
    _scene->use3dCursor(false);
// M$Windows generates MouseLeave events constantly (every frame ?)
#ifndef WIN32
    if (TheApp->useStereo())
        swSetCursor(_wnd, _cursorArrow);
#endif
    swInvalidateWindow(_wnd);
} 

void Scene3DView::OnMouseMove(int x, int y, int modifiers) 
{
#ifdef WIN32
    if (!(_trackMouse) || (_hitHandle))
        if (TheApp->useStereo())
            swSetCursor(_wnd, _cursorNone);
#endif

    int	    handle = _scene->getSelectedHandle();
    int	    width, height;

    swMakeCurrent(_dc, _glc);
    swGetSize(_wnd, &width, &height);

    if (TheApp->useStereo() && _scene->use3dCursor()) {
       _mouseX = x;
       _mouseY = y;
       swInvalidateWindow(_wnd);
    }

    if (_trackMouse) {
	float dx = (float) x - _trackX;
	float dy = (float) y - _trackY;
	if (modifiers & SW_CONTROL) {
            if (modifiers & SW_SHIFT) {
 	        swSetCursor(_wnd, _cursorMove);
                _scene->moveCamera(-dx * 0.05f, dy * 0.05f, 0.0f);
                _trackX = x;
                _trackY = y;
                swInvalidateWindow(_wnd);
            } else {
  	        swSetCursor(_wnd, _cursorRotate);
	        _scene->orbitCamera(-DEG2RAD(dx * 0.5f), -DEG2RAD(dy * 0.5f));
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
            }
	} else if (modifiers & SW_SHIFT) {
 	    swSetCursor(_wnd, _cursorZoom);
	    _scene->moveCamera(0.0f, 0.0f, dy * 0.25f);
	    _trackX = x;
	    _trackY = y;
    	    swInvalidateWindow(_wnd);
	} else if (modifiers & SW_ALT) {
             /* SGI style ALT key navigation */
             if (_button1down && _button2down)
                {
 	        swSetCursor(_wnd, _cursorZoom);
   	        _scene->moveCamera(0.0f, 0.0f, dy * 0.25f);
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
                }
             else if (_button1down)
                {
 	        swSetCursor(_wnd, _cursorRotate);
   	        _scene->orbitCamera(-DEG2RAD(dx * 0.5f), -DEG2RAD(dy * 0.5f));
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
                }
             else if (_button2down)
                {
 	        swSetCursor(_wnd, _cursorMove);
   	        _scene->moveCamera(-dx * 0.05f, dy * 0.05f, 0.0f);
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
                }
	} else if (handle != -1) {
	    Node   *node = _scene->getSelection()->getNode();
	    int		constraint = CONSTRAIN_NONE, field;
	    Vec3f	old = node->getHandle(handle, &constraint, &field);
            if (field==-1) {
                _scene->setSelectedHandle(-1);
                return;
            }
	    Vec3f	v = old;
	    if (constraint == CONSTRAIN_NONE) {
		float px, py, pz;
		glPushMatrix();
		glLoadIdentity();
		_scene->transform(_scene->getSelection());
		node->transformForHandle(handle);
		_scene->projectPoint(old.x, old.y, old.z, &px, &py, &pz);
		_scene->unProjectPoint((float) x, (float) height - y, pz, &v.x, &v.y, &v.z);
		glPopMatrix();
		v += _offset;
	    } else {
		float x1, y1, z1;
		float x2, y2, z2;
		Matrix mat;
		glPushMatrix();
		glLoadIdentity();
		_scene->transform(_scene->getSelection());
		glGetFloatv(GL_MODELVIEW_MATRIX, mat);
		node->transformForHandle(handle);
		_scene->unProjectPoint((float) x, (float) height - y, 0.0f, &x1, &y1, &z1);
		_scene->unProjectPoint((float) x, (float) height - y, 1.0f, &x2, &y2, &z2);
		glPopMatrix();

		x1 += _offset.x;
		y1 += _offset.y;
		z1 += _offset.z;
		x2 += _offset.x;
		y2 += _offset.y;
		z2 += _offset.z;

		switch (constraint) {
		  case CONSTRAIN_X:
		    y1 -= v.y;
		    z1 -= v.z;
		    y2 -= v.y;
		    z2 -= v.z;
		    v.x = constrainLine(y1, z1, x1, y2, z2, x2);
		    break;
		  case CONSTRAIN_Y:
		    x1 -= v.x;
		    z1 -= v.z;
		    x2 -= v.x;
		    z2 -= v.z;
		    v.y = constrainLine(x1, z1, y1, x2, z2, y2);
		    break;
		  case CONSTRAIN_Z:
		    x1 -= v.x;
		    y1 -= v.y;
		    x2 -= v.x;
		    y2 -= v.y;
		    v.z = constrainLine(x1, y1, z1, x2, y2, z2);
		    break;
		  case CONSTRAIN_XY:
		    constrainPlane(x1, y1, z1, x2, y2, z2, &v.x, &v.y);
		    v.z = 0.0f;
		    break;
		  case CONSTRAIN_SPHERE:
		    v = constrainSphere(v, dx, dy, mat);
		    _trackX = x;
		    _trackY = y;
		    break;
		}
	    }
	    if (v != old) {
		if (!_backedUp) {
                    _scene->backupFieldsStart();
		    _scene->backupFieldsAppend(node, field);
                    if (node->getType() == NODE_NURBS_SURFACE)  
                        if (_scene->getXSymetricNurbsMode()) {
                            NodeNurbsSurface *surf = (NodeNurbsSurface *) node;
                            NodeNurbsGroup *nurbsGroup = surf->findNurbsGroup();
                            if (nurbsGroup != NULL)
                                nurbsGroup->backupFieldsAppend(node, field);
                        }
                    _scene->backupFieldsDone();
		    _backedUp = true;
		}
		node->setHandle(handle, v);
	    }
        } else if (_scene->getNavigationMode()) {
             if (_button1down && _button2down)
                {
  	        swSetCursor(_wnd, _cursorMove);
   	        _scene->moveCamera(-dx * 0.05f, dy * 0.05f, 0.0f);
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
                }
             else if (_button1down)
                {
  	        swSetCursor(_wnd, _cursorRotate);
   	        _scene->orbitCamera(-DEG2RAD(dx * 0.5f), -DEG2RAD(dy * 0.5f));
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
                }
             else if (_button2down)
                {
  	        swSetCursor(_wnd, _cursorZoom);
   	        _scene->moveCamera(0.0f, 0.0f, dy * 0.25f);
	        _trackX = x;
	        _trackY = y;
    	        swInvalidateWindow(_wnd);
                }
	}
    }
}

void Scene3DView::OnLButtonUp(int x, int y, int modifiers) 
{
    _trackMouse = false;
    if (TheApp->useStereo())
        swSetCursor(_wnd, _cursorNone);
    else
        swSetCursor(_wnd, _cursorArrow);
    swReleaseCapture(_wnd);
    _scene->setSelectedHandle(-1);
    _button1down = false;
    if (_button2down)
       {
       swMakeCurrent(_dc, _glc);
       swSetFocus(_wnd);

       if (modifiers & SW_ALT)
          swSetCursor(_wnd, _cursorMove);

       _trackMouse = true;
       swSetCapture(_wnd);
       _trackX = x;
       _trackY = y;
       }

}

void Scene3DView::OnMButtonUp(int x, int y, int modifiers) 
{
    _trackMouse = false;
    if (TheApp->useStereo())
        swSetCursor(_wnd, _cursorNone);
    else
        swSetCursor(_wnd, _cursorArrow);
    swReleaseCapture(_wnd);
    _scene->setSelectedHandle(-1);
    _button2down = false;
    if (_button1down)
       {
       swMakeCurrent(_dc, _glc);
       swSetFocus(_wnd);

       if (modifiers & SW_ALT)
          swSetCursor(_wnd, _cursorRotate);

       _trackMouse = true;
       swSetCapture(_wnd);
       _trackX = x;
       _trackY = y;
       }
}

Quaternion &Scene3DView::getQuat(const Path* path)
   {
   static Quaternion ret; 
   // initialise with unit one quaternion
   ret= Quaternion(0.0 , 0.0 , 0.0 , 1.0);
   const NodeList    *nodes = path->getNodes();

   for (int i = (nodes->size()-2) ; i >= 0 ; i--) {
       if (nodes->get(i)->getType()==NODE_TRANSFORM) {
          NodeTransform* transform=(NodeTransform*)nodes->get(i);
          Quaternion q=transform->getQuat();
          q.normalize();
          ret=ret*q;
       }
   }
   return ret;
}

void Scene3DView::Transform3D(const Path* path,InputDevice* inputDevice)
{
    glPushMatrix();
    glLoadIdentity();
    Quaternion viewrot=_scene->getCamera()->getOrientation();
    viewrot.normalize();
    NodeTransform* transform=(NodeTransform*)path->getNode();
    TransformMode* tm=_scene->getTransformMode();
    Quaternion oldrot=transform->getQuat();
    float norm_oldrot=oldrot.norm();
    oldrot.normalize();
    Quaternion scenerot=getQuat(path);
    Quaternion newrot;
    bool rotation_changed=false;
    bool translation_changed=false;
    bool scale_changed=false;
    bool center_changed=false;
    if (tm->hasRotation()) {
       rotation_changed=true; 
       if (tm->isLocal()) {
          Quaternion inputrot=inputDevice->get_localQuaternion(tm);
          newrot=inputrot*oldrot;
       } else {
          Quaternion inputrot=inputDevice->get_quaternion(tm);
          newrot=oldrot*
                 (scenerot*viewrot.conj()*inputrot*viewrot*scenerot.conj());
       }
       transform->setQuat(newrot*norm_oldrot);
       _scene->setField(transform, _transRotationField, new SFRotation(
                                                       newrot*norm_oldrot));
    }
    if (tm->hasTranslation()) {
       Vec3f v=inputDevice->get_vector(tm);
       if ((v[0]!=0) && (v[1]!=0) && (v[2]!=0))
          translation_changed=true;
       Vec3f xyz;
       if (!tm->isLocal()) {
          xyz=viewrot*scenerot.conj()*oldrot.conj()*v;
          v[0]= xyz.x;
          v[1]= xyz.y;
          v[2]= xyz.z;
       } else {
          xyz=(scenerot*v)*scenerot.conj();
          v[0]=-xyz.x;
          v[1]= xyz.y;
          v[2]=-xyz.z;
       }
       transform->setHandle(TRANSLATION,v);
    } else if (tm->tmode==TM_SCALE) {
       Vec3f oldscale=transform->getScale();
       Vec3f v=inputDevice->get_vector(tm);
       if ((v[0]!=0) && (v[1]!=0) && (v[2]!=0))
          scale_changed=true;
       transform->setHandle(SCALE,v+oldscale);
    } else if (tm->tmode==TM_CENTER) {
       Vec3f v=inputDevice->get_vector(tm);
       if ((v[0]!=0) && (v[1]!=0) && (v[2]!=0))
          center_changed=true;
       transform->setHandle(CENTER,(viewrot*oldrot.conj())*v);
    }
    // Backup of inputdevice data is maybe memory murder 8-(
    if (translation_changed)
       _scene->backupField(path->getNode(), _transRotationField);
    if (rotation_changed)
       _scene->backupField(path->getNode(), _transTranslationField);
    if (scale_changed)
       _scene->backupField(path->getNode(), _transScaleField);
    if (center_changed)
       _scene->backupField(path->getNode(), _transCenterField);
    if (   (translation_changed) || (rotation_changed) 
        || (scale_changed) || (center_changed))
       swInvalidateWindow(_wnd);
    glPopMatrix();
}

void Scene3DView::Handle3D(const Path* path,InputDevice* inputDevice, 
                           int handle)
{
    int		constraint = CONSTRAIN_NONE, field;
    Node*       node = path->getNode();
    Vec3f	old = node->getHandle(handle, &constraint, &field);
    if (field==-1) {
        _scene->setOldSelectedHandle(-1);
        return;
    }
    Vec3f	vNew = old;
    glPushMatrix();
    glLoadIdentity();
    _scene->transform(_scene->getSelection());
    node->transformForHandle(handle);
    glPopMatrix();
    TransformMode* tm=_scene->getTransformMode();
    Quaternion viewrot=_scene->getCamera()->getOrientation();
    Quaternion oldrot(0,0,0,1);
    float norm_oldrot=oldrot.norm();
    oldrot.normalize();
    Quaternion scenerot=getQuat(path);
    Quaternion newrot;
    if (tm->hasTranslation()) {
        Vec3f v=inputDevice->get_vector(tm);
        Vec3f xyz;
        if (!tm->isLocal()) {
            xyz=viewrot*scenerot.conj()*oldrot.conj()*v;
            v[0]= xyz.x;
            v[1]= xyz.y;
            v[2]= xyz.z;
        } else {
            xyz=(scenerot*v)*scenerot.conj();
            v[0]=-xyz.x;
            v[1]= xyz.y;
            v[2]=-xyz.z;
        }
        vNew.x+=v[0];
        vNew.y+=v[1];
        vNew.z+=v[2];
    }
    glPopMatrix();
    if (vNew != old) {
        if (!_backedUp) {
            _scene->backupField(node, field);
            _backedUp = true;
        }
        node->setHandle( handle, vNew);
    } 
}

void Scene3DView::Navigate3D(InputDevice* inputDevice)
{
    glPushMatrix();
    glLoadIdentity();
    Quaternion viewrot=_scene->getCamera()->getOrientation();
    Quaternion newrot;
    bool rotation_changed=false;
    bool translation_changed=false;
    TransformMode* tm=_scene->getTransformMode();
    if (!inputDevice->getHeadNavigation()) 
       {
       TransformMode tmTracker(TM_TRANSLATE,TM_3D,TM_NEAR_FAR);
       tm=&tmTracker;
       }
    if (tm->hasRotation()) {
       rotation_changed=true; 
       Quaternion inputrot=inputDevice->get_quaternion(tm);
       inputrot.z=-inputrot.z;
       newrot=inputrot*viewrot; 
       _scene->getCamera()->setOrientation(newrot);
    }
    if (tm->hasTranslation()) {
       Vec3f v= inputDevice->get_vector(tm);
       if ((v[0]!=0) && (v[1]!=0) && (v[2]!=0))
          translation_changed=true;
       Vec3f xyz;
       if ((inputDevice->isTracker()) || (inputDevice->isWand())) {
           xyz=newrot.conj()*(viewrot*v);
       } else
           xyz=viewrot*v;
       v[0]=xyz.x;
       v[1]=xyz.y;
       v[2]=xyz.z;
       Vec3f vcamera=_scene->getCamera()->getPosition();
       v[0]=v[0]+vcamera.x;
       v[1]=v[1]+vcamera.y;
       v[2]=v[2]+vcamera.z;
       _scene->getCamera()->setPosition(v);
    }
    _scene->applyCamera();
    swInvalidateWindow(_wnd);
    // Backup of inputdevice data is maybe memory murder 8-(
    if (translation_changed)
       _scene->backupField(_scene->getCamera(), _viewPositionField);
    if (rotation_changed)
       _scene->backupField(_scene->getCamera(), _viewOrientationField);
    glPopMatrix();
}

// read from a array of InputDevice devices and move/rotate selected thing

int 
Scene3DView::readInputDevice(void)
{
    if ((TheApp->getNumberInputDevices()>0) && (!_fieldEdit)) {
      if (_scene->getHasFocus()) {
        int i;
        for (i=0;i<TheApp->getNumberInputDevices();i++)
          if (TheApp->getInputDevice(i)->hasReadDelay())
            TheApp->getInputDevice(i)->prepareRead();
        for (i=0;i<TheApp->getNumberInputDevices();i++) {
          InputDevice* inputDevice=TheApp->getInputDevice(i);
          if (inputDevice==NULL)
             fprintf(stderr,"internal error in Scene3DView::readInputDevice\n");
          if (inputDevice->readInputDevice() && !(inputDevice->allzero())) {
             int handle = _scene->getOldSelectedHandle();
             const Path   *path = _scene->getSelection();
    	     Node *node = path->getNode();
             Path *transform = NULL;
             if ((node->getType()==NODE_TRANSFORM) && !inputDevice->isHead()) {
                Transform3D(path,inputDevice);
             } else if ((handle != -1) && !inputDevice->isHead()) {
                Handle3D(path,inputDevice, handle);
    	     } else if ((_scene->getNavigationMode()) && !inputDevice->isWand())
                Navigate3D(inputDevice);
    	     else if ((node==_scene->getRoot()) && !inputDevice->isWand())
                Navigate3D(inputDevice);
    	     else if ((node->getType()==NODE_VIEWPOINT) && 
                      !inputDevice->isWand())
                Navigate3D(inputDevice);
             else if (( (transform = _scene->searchTransform()) != NULL)
                      &&
                      (!inputDevice->sendalways())) {
                _scene->setSelection(transform);
	        _scene->UpdateViews(NULL, UPDATE_SELECTION);
                _scene->setSelectedHandle(-1);
                Transform3D(_scene->getSelection(),inputDevice);
                transform=NULL;
    	     } else if (inputDevice->isHead())
                Navigate3D(inputDevice);
    	  }
        }
      }
#ifndef WIN32	  
      if (_InputDeviceTimer) {
        swKillTimer(_InputDeviceTimer);
        _InputDeviceTimer=NULL;
      }
#endif
      _InputDeviceTimer = swSetTimer(_wnd, INPUTDEVICE_TIME, 
                                   InputDeviceTimerCallback, this);

    }
    return(0);
}

float Scene3DView::constrainLine(float x1, float y1, float z1, float x2, float y2, float z2) const
{
    float	dx = x1 - x2;
    float	dy = y1 - y2;
    if (dx == 0.0f && dy == 0.0f) 
	return z2;
    float	alpha = (x1 * dx + y1 * dy) / (dx * dx + dy * dy);

    return (1.0f - alpha) * z1 + alpha * z2;
}

void Scene3DView::constrainPlane(float x1, float y1, float z1, float x2, float y2, float z2, float *x, float *y) const
{
    if(EQUALF(z1, z2)) {
	*x = *y = 0.0f;
	return;
    }

    float	alpha = -z1 / (z2 - z1);
    
    *x = (1.0f - alpha) * x1 + alpha * x2;
    *y = (1.0f - alpha) * y1 + alpha * y2;
}

Vec3f Scene3DView::constrainSphere(const Vec3f &euler, float dx, float dy,
				   const Matrix &mat)
{
    SFRotation	sfr(euler, 0);
    Quaternion	cam = _scene->getCamera()->getOrientation();

    Quaternion  ry(Vec3f(0.0f, 1.0f, 0.0f), DEG2RAD(dx));
    Quaternion	rx(Vec3f(1.0f, 0.0f, 0.0f), DEG2RAD(dy));
    Quaternion  r = sfr.getQuat() * cam.conj() * ry * rx * cam;
    return SFRotation(r).getEulerAngles(0);
}