Nebula
Toggle main menu visibility
Loading...
Searching...
No Matches
pquatfeedbackloop.h
Go to the documentation of this file.
1
#ifndef Math_PQUATFEEDBACKLOOP_H
2
#define Math_PQUATFEEDBACKLOOP_H
3
//------------------------------------------------------------------------------
13
#include "
core/types.h
"
14
#include "math/quaternion.h"
15
#include "
math/vector.h
"
16
#include "
timing/time.h
"
17
18
namespace
Math
19
{
20
using namespace
Timing
;
21
22
//------------------------------------------------------------------------------
23
class
PQuatFeedbackLoop
24
{
25
public
:
27
PQuatFeedbackLoop
();
29
void
Reset(
Time
time,
float
stepSize,
float
gain,
const
quaternion& curState);
31
void
SetGain(
float
g);
33
float
GetGain()
const
;
35
void
SetGoal(
const
quaternion& c);
37
const
quaternion& GetGoal()
const
;
39
void
SetState(
const
quaternion& s);
41
const
quaternion& GetState()
const
;
43
void
Update(
Time
time);
44
45
private
:
46
Time
time;
// the time at which the simulation is
47
float
stepSize;
48
float
gain;
49
quaternion goal;
50
quaternion state;
51
};
52
53
//------------------------------------------------------------------------------
56
inline
57
PQuatFeedbackLoop::PQuatFeedbackLoop() :
58
time(0.0),
59
stepSize(0.001f),
60
gain(-1.0f)
61
{
62
// empty
63
}
64
65
//------------------------------------------------------------------------------
68
inline
69
void
70
PQuatFeedbackLoop::Reset(
Time
t,
float
s,
float
g,
const
quaternion& curState)
71
{
72
this->time = t;
73
this->stepSize = s;
74
this->gain = g;
75
this->state = curState;
76
}
77
78
//------------------------------------------------------------------------------
81
inline
82
void
83
PQuatFeedbackLoop::SetGain(
float
g)
84
{
85
n_assert
(g > 0.0f);
86
this->gain = g;
87
}
88
89
//------------------------------------------------------------------------------
92
inline
93
float
94
PQuatFeedbackLoop::GetGain()
const
95
{
96
return
this->gain;
97
}
98
99
//------------------------------------------------------------------------------
102
inline
103
void
104
PQuatFeedbackLoop::SetGoal(
const
quaternion& g)
105
{
106
this->goal = g;
107
}
108
109
//------------------------------------------------------------------------------
112
inline
113
const
quaternion&
114
PQuatFeedbackLoop::GetGoal()
const
115
{
116
return
this->goal;
117
}
118
119
//------------------------------------------------------------------------------
122
inline
123
void
124
PQuatFeedbackLoop::SetState(
const
quaternion& s)
125
{
126
this->state = s;
127
}
128
129
//------------------------------------------------------------------------------
132
inline
133
const
quaternion&
134
PQuatFeedbackLoop::GetState()
const
135
{
136
return
this->state;
137
}
138
139
//------------------------------------------------------------------------------
142
inline
143
void
144
PQuatFeedbackLoop::Update
(
Time
curTime)
145
{
146
Time
dt = curTime - this->time;
147
148
// catch time exceptions
149
if
(dt < 0.0)
150
{
151
this->time = curTime;
152
}
153
else
if
(dt > 0.5)
154
{
155
this->time = curTime;
156
}
157
158
while
(this->time < curTime)
159
{
160
// compute angular error between state and goal
161
vector stateVec = this->state.rotate(vector(0.0f, 0.0f, 1.0f));
162
vector goalVec = this->goal.rotate(vector(0.0f, 0.0f, 1.0f));
163
float
dot = stateVec % goalVec;
164
165
// dot is now between -1 (opposite vectors) and 1 (identical vectors),
166
// generate a lerp value between 0 (represents goal) and 1 (represents state)
167
float
error = (-dot + 1.0f) * 0.5f;
168
169
// error now 0 for "no error" and 1 for "max error"
170
float
lerp = error * this->gain * this->stepSize;
171
quaternion res;
172
res.slerp(this->state, this->goal, lerp);
173
this->state = res;
174
175
this->time += this->stepSize;
176
}
177
}
178
179
//------------------------------------------------------------------------------
180
#endif
PQuatFeedbackLoop
Definition
PQuatFeedbackLoop.cs:11
PQuatFeedbackLoop.Update
Quaternion Update(double dt)
Perform one step/loop/update of the PID controller.
Definition
PQuatFeedbackLoop.cs:32
n_assert
#define n_assert(exp)
Definition
debug.h:58
Math
Different curves.
Definition
angularpfeedbackloop.h:17
Timing
Definition
calendartimebase.h:19
Timing::Time
double Time
the time datatype
Definition
time.h:18
time.h
Typedefs for the Timing subsystem.
types.h
vector.h
code
foundation
math
pquatfeedbackloop.h
Generated on
for Nebula. Dark theme by
Tilen Majerle
. All rights reserved.