2
* $Id: SM_MotionState.cpp,v 1.2 2004/12/04 09:51:04 kester Exp $
4
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
6
* This program is free software; you can redistribute it and/or
7
* modify it under the terms of the GNU General Public License
8
* as published by the Free Software Foundation; either version 2
9
* of the License, or (at your option) any later version. The Blender
10
* Foundation also sells licenses for use in proprietary software under
11
* the Blender License. See http://www.blender.org/BL/ for information
14
* This program is distributed in the hope that it will be useful,
15
* but WITHOUT ANY WARRANTY; without even the implied warranty of
16
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17
* GNU General Public License for more details.
19
* You should have received a copy of the GNU General Public License
20
* along with this program; if not, write to the Free Software Foundation,
21
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
24
* All rights reserved.
26
* The Original Code is: all of this file.
28
* Contributor(s): none yet.
30
* ***** END GPL/BL DUAL LICENSE BLOCK *****
32
#include <MT_Scalar.h>
33
#include <MT_Vector3.h>
34
#include <MT_Quaternion.h>
36
#include "SM_MotionState.h"
38
void SM_MotionState::integrateMidpoint(MT_Scalar timeStep, const SM_MotionState &prev_state, const MT_Vector3 &velocity, const MT_Quaternion& ang_vel)
40
m_pos += (prev_state.getLinearVelocity() + velocity) * (timeStep * 0.5);
41
m_orn += (prev_state.getAngularVelocity() * prev_state.getOrientation() + ang_vel * m_orn) * (timeStep * 0.25);
45
void SM_MotionState::integrateBackward(MT_Scalar timeStep, const MT_Vector3 &velocity, const MT_Quaternion& ang_vel)
47
m_pos += velocity * timeStep;
48
m_orn += ang_vel * m_orn * (timeStep * 0.5);
52
void SM_MotionState::integrateForward(MT_Scalar timeStep, const SM_MotionState &prev_state)
54
m_pos += prev_state.getLinearVelocity() * timeStep;
55
m_orn += prev_state.getAngularVelocity() * m_orn * (timeStep * 0.5);
60
// Newtonian lerp: interpolate based on Newtonian motion
61
void SM_MotionState::nlerp(const SM_MotionState &prev, const SM_MotionState &next)
63
MT_Scalar dt = next.getTime() - prev.getTime();
64
MT_Scalar t = getTime() - prev.getTime();
65
MT_Vector3 dx = next.getPosition() - prev.getPosition();
66
MT_Vector3 a = dx/(dt*dt) - prev.getLinearVelocity()/dt;
68
m_pos = prev.getPosition() + prev.getLinearVelocity()*t + a*t*t;
72
void SM_MotionState::lerp(const SM_MotionState &prev, const SM_MotionState &next)
74
MT_Scalar dt = next.getTime() - prev.getTime();
81
MT_Scalar x = (getTime() - prev.getTime())/dt;
83
m_pos = x*next.getPosition() + (1-x)*prev.getPosition();
85
m_orn = prev.getOrientation().slerp(next.getOrientation(), 1-x);
87
m_lin_vel = x*next.getLinearVelocity() + (1-x)*prev.getLinearVelocity();
88
m_ang_vel = x*next.getAngularVelocity() + (1-x)*prev.getAngularVelocity();
91
void SM_MotionState::lerp(MT_Scalar t, const SM_MotionState &other)
93
MT_Scalar x = (t - getTime())/(other.getTime() - getTime());
94
m_pos = (1-x)*m_pos + x*other.getPosition();
96
m_orn = other.getOrientation().slerp(m_orn, x);
98
m_lin_vel = (1-x)*m_lin_vel + x*other.getLinearVelocity();
99
m_ang_vel = (1-x)*m_ang_vel + x*other.getAngularVelocity();