mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-30 04:06:33 +08:00
Functions: add squareroot linear function
This commit is contained in:
@@ -172,4 +172,30 @@ const T gradual3(const T &value,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Squareroot, linear function with fixed corner point at intersection (1,1)
|
||||||
|
* /
|
||||||
|
* linear /
|
||||||
|
* /
|
||||||
|
* 1 /
|
||||||
|
* /
|
||||||
|
* sqrt |
|
||||||
|
* |
|
||||||
|
* 0 -------
|
||||||
|
* 0 1
|
||||||
|
*/
|
||||||
|
template<typename T>
|
||||||
|
const T sqrt_linear(const T &value)
|
||||||
|
{
|
||||||
|
if (value < static_cast<T>(0)) {
|
||||||
|
return static_cast<T>(0);
|
||||||
|
|
||||||
|
} else if (value < static_cast<T>(1)) {
|
||||||
|
return sqrtf(value);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
} /* namespace math */
|
} /* namespace math */
|
||||||
|
|||||||
@@ -183,3 +183,14 @@ TEST(FunctionsTest, gradual3)
|
|||||||
0.f, .5f, 1.5f,
|
0.f, .5f, 1.5f,
|
||||||
1.f, 2.f, 3.f), 3.f);
|
1.f, 2.f, 3.f), 3.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(FunctionsTest, sqrt_linear)
|
||||||
|
{
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(-12.f), 0.f);
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(-2.f), 0.f);
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(0.f), 0.f);
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(.5f), 0.70710678f);
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(1.f), 1.f);
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(2.f), 2.f);
|
||||||
|
EXPECT_FLOAT_EQ(sqrt_linear(120.f), 120.f);
|
||||||
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user