/// <summary>
/// TODO: LDPoint注意
/// </summary>
/// <param name="pt"></param>
/// <param name="clip"></param>
/// <returns></returns>
public LDPoint inverseTransform(LDPoint pt, bool clip = false)
{
if (getColumn() == 0 && getRow() == 0)
{
//メッシュがない場合はそのままの値を返す
return(pt);
}
double x = pt.x();
double y = pt.y();
//Gridからマッピング先のQuadを見つける
LDPoint index = getMapedQuadIndex(new LDPoint((float)x, (float)y));
int col = (int)index.x();
int row = (int)index.y();
if (clip)
{
//Grid内にClipする。QuadをClipして変換するのではない
col = LDMathUtil.clamp(col, 0, getColumn());
row = LDMathUtil.clamp(row, 0, getRow());
}
LDPoint local = new LDPoint();
if (isOutside(row, col))
{
LDQuadTransform quad = createOutsideQuadTransform(row, col);
return(quad.inverseTransform(pt, clip));
//TODO:到達しない場所あります
local = quad.inverseTransform(pt, clip);
LDPoint _internal = new LDPoint();
_internal.setX((col + 1) * (1.0f / (getColumn() + 2)) + local.x() / (getColumn() + 2));
_internal.setY((row + 1) * (1.0f / (getRow() + 2)) + local.y() / (getRow() + 2));
LDQuadTransform tmp = new LDQuadTransform(new LDPoint(-1, -1), new LDPoint(getColumn() + 1, getRow() + 1));
return(tmp.transform(_internal));
}
else
{
LDQuadTransform quad = getQuadTransform(row, col);
local = quad.inverseTransform(pt, clip);
LDPoint _internal = new LDPoint();
_internal.setX(col * (1.0f / getColumn()) + local.x() / getColumn());
_internal.setY(row * (1.0f / getRow()) + local.y() / getRow());
return(_internal);
}
}
public void wrapPiTest()
{
ld_float actual;
ld_float expected;
ld_float theta;
//ld_float delta = 0.00001f;
// Input : 0.0
theta = 0.0f;
expected = 0.0f;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
// Input : 2PI
theta = LDMathUtil.PI2;
expected = 0.0f;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
// Input : PI, PI±0.01におけるラップテスト
theta = LDMathUtil.PI - 0.01f;
expected = LDMathUtil.PI - 0.01f;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
theta = LDMathUtil.PI;
expected = LDMathUtil.PI;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
theta = LDMathUtil.PI + 0.01f;
expected = 0.01f - LDMathUtil.PI;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
// Input : -PI, -PI±0.01におけるラップテスト
theta = -LDMathUtil.PI - 0.01f;
expected = LDMathUtil.PI - 0.01f;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
theta = -LDMathUtil.PI;
expected = -LDMathUtil.PI;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
theta = -LDMathUtil.PI + 0.01f;
expected = -LDMathUtil.PI + 0.01f;
actual = LDMathUtil.wrapPi(theta);
TestUtil.COMPARE(expected, actual);
}
/**
* @brief 慣性空間->オブジェクト空間への回転
* @param &orientation オイラー角(EEulerAngles)をセットする
*/
void setToRotateInertialToObject(LDEulerAngles orientation)
{
ld_float sp, sb, sh;
ld_float cp, cb, ch;
LDMathUtil.sinCos(out sp, out cp, orientation.pitch * 0.5f);
LDMathUtil.sinCos(out sb, out cb, orientation.bank * 0.5f);
LDMathUtil.sinCos(out sh, out ch, orientation.heading * 0.5f);
w = ch * cp * cb + sh * sp * sb;
x = -ch * sp * cb - sh * cp * sb;
y = ch * sp * sb - sh * cp * cb;
z = sh * sp * cb - ch * cp * sb;
}
public void updateUvMap(LDSize ImageSize)
{
m_uvMap.Clear();
for (int i = 0; i < m_points.size(); i++)
{
float u = m_points[i].x() / ImageSize.width();
float v = m_points[i].y() / ImageSize.height();
LDMathUtil.clamp(u, 0.0f, 1.0f);
LDMathUtil.clamp(v, 0.0f, 1.0f);
m_uvMap.Add(new LDPoint(u, v));
}
}
//追加。UV経由での追加しかできない。Positionは自動で計算される
public void addUv(LDPoint uv, bool clamp = true)
{
if (clamp)
{
//0-1区間の縛りに引っかかる処理が多いので、デフォルトでは自動で直す
uv = LDMathUtil.clamp(uv, new LDPoint(0, 0), new LDPoint(1, 1));
}
else
{
common.LD_ASSERT_OUT_OF_RANGE(uv.x(), 0, 1);
common.LD_ASSERT_OUT_OF_RANGE(uv.y(), 0, 1);
}
m_uvMap.Add(uv);
//頂点の計算
m_points.Add(getCalcAddVertex(uv));
}
public void getAngleDiffTest()
{
ld_float actual;
ld_float expected;
ld_float Q1, Q2;
//ld_float delta = 0.00001f;
// Input : Q1 = 0.0, Q2 = 0.0
Q1 = 0.0f;
Q2 = 0.0f;
expected = 0.0f;
actual = LDMathUtil.getAngleDiff(Q1, Q2);
TestUtil.COMPARE(expected, actual);
// Input : Q1 = PI, Q2 = 0.0
Q1 = LDMathUtil.PI;
Q2 = 0.0f;
expected = LDMathUtil.PI;
actual = LDMathUtil.getAngleDiff(Q1, Q2);
TestUtil.COMPARE(expected, actual);
// Input : Q1 = PI+0.1, Q2 = 0.0
Q1 = LDMathUtil.PI + 0.1f;
Q2 = 0.0f;
expected = -LDMathUtil.PI + 0.1f;
actual = LDMathUtil.getAngleDiff(Q1, Q2);
TestUtil.COMPARE(expected, actual);
// Input : Q1 = -PI, Q2 = 0.0
Q1 = -LDMathUtil.PI;
Q2 = 0.0f;
expected = -LDMathUtil.PI;
actual = LDMathUtil.getAngleDiff(Q1, Q2);
TestUtil.COMPARE(expected, actual);
// Input : Q1 = -PI-0.1, Q2 = 0.0
Q1 = -LDMathUtil.PI - 0.1f;
Q2 = 0.0f;
expected = LDMathUtil.PI - 0.1f;
actual = LDMathUtil.getAngleDiff(Q1, Q2);
TestUtil.COMPARE(expected, actual);
}
public void safeAcosTest()
{
ld_float actual;
ld_float expected;
ld_float x;
//ld_float delta = 0.00001f;
// Input : x = -1.0f
x = -1.0f;
expected = LDMathUtil.PI;
actual = LDMathUtil.safeAcos(x);
TestUtil.COMPARE(expected, actual);
// Input : x = 1.0f
x = 1.0f;
expected = 0.0f;
actual = LDMathUtil.safeAcos(x);
TestUtil.COMPARE(expected, actual);
}
public LDPoint transform(double tx, double ty, bool clip = false)
{
if (clip)
{
tx = LDMathUtil.clamp(tx, 0.0, 1.0);
ty = LDMathUtil.clamp(ty, 0.0, 1.0);
}
/*
* if ( tx + ty < 1 )
* {
* //左上三角
* LDTriangleTransform tr(m_topLeft,m_topRight,m_bottomLeft);
* return tr.transform(tx,ty);
* }
* else
* {
* //右下三角
* //TODO SDKでは計算式を(1-t)にしている。違いがある?
* // LDTriangleTransform tr(m_bottomLeft,m_bottomRight,m_topRight);
* // return tr.transform(tx,ty);
* LDTriangleTransform tr(m_topRight,m_bottomLeft,m_bottomRight);
* return tr.transformOneMinusT(tx,ty);
* }
*/
double x = LDMathUtil.lerp2D(
m_topLeft.x(), m_topRight.x(), tx,
m_bottomLeft.x(), m_bottomRight.x(), ty
);
double y = LDMathUtil.lerp2D(
m_topLeft.y(), m_topRight.y(), tx,
m_bottomLeft.y(), m_bottomRight.y(), ty
);
return(new LDPoint((float)x, (float)y));
}
public LDPoint inverseTransform(double x, double y, bool clip = false)
{
/*
* QPolygonF p;
* p.Add(m_topLeft);
* p.Add(m_topRight);
* p.Add(m_bottomLeft);
* // p<<m_topLeft<<m_topRight<<m_bottomLeft;
*
* if ( p.containsPoint(LDPoint(x,y),Qt.WindingFill))
* {
* //左上三角
* LDTriangleTransform tr(m_topLeft,m_topRight,m_bottomLeft);
* return tr.inverseTransform(x,y);
* }
* else
* {
* //右下三角
* //TODO SDKでは計算式を(1-t)にしている。違いがある?
* // LDTriangleTransform tr(m_bottomLeft,m_bottomRight,m_topRight);
* // return tr.transform(tx,ty);
* LDTriangleTransform tr(m_topRight,m_bottomLeft,m_bottomRight);
* return tr.inverseTransformOneMinusT(x,y);
* }
*/
double tx, ty;
LDMathUtil.inverseLerp2D(m_topLeft, m_topRight, m_bottomLeft, m_bottomRight, new LDPoint((float)x, (float)y), out tx, out ty);
if (clip)
{
tx = LDMathUtil.clamp(tx, 0.0, 1.0);
ty = LDMathUtil.clamp(ty, 0.0, 1.0);
}
return(new LDPoint((float)tx, (float)ty));
}
//平面上の点を変換した結果を返す。pは基本0..1の範囲
/// <summary>
/// TODO:LDPoint注意
/// </summary>
/// <param name="t"></param>
/// <param name="clip"></param>
/// <returns></returns>
public LDPoint transform(LDPoint t, bool clip = false)
{
if (getColumn() == 0 && getRow() == 0)
{
//メッシュがない場合はそのままの値を返す
return(t);
}
double tx = t.x();
double ty = t.y();
if (clip)
{
//Grid内にClipする。QuadをClipして変換するのではない
tx = LDMathUtil.clamp(tx, 0.0, 1.0);
ty = LDMathUtil.clamp(ty, 0.0, 1.0);
}
//Gridから対象のQuadを見つける
LDPoint index = getQuadIndex(new LDPoint((float)tx, (float)ty));
int col = (int)index.x();
int row = (int)index.y();
LDPoint local = getLocalPoint(new LDPoint((float)tx, (float)ty));
if (isOutside(row, col))
{
LDQuadTransform quad = createOutsideQuadTransform(row, col);
return(quad.transform(new LDPoint((float)tx, (float)ty)));
}
else
{
LDQuadTransform quad = getQuadTransform(row, col);
return(quad.transform(local));
}
}
//--------------------
/**
* @brief 回転角を取り出して返す
*/
public ld_float getAngle()
{
return(LDMathUtil.safeAcos(w) * 2.0f);
}
public void mySlerpTest()
{
LDQuat quat1 = new LDQuat();
LDQuat quat2 = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float t;
// Input : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 0.0
// 範囲外t <= 0.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 0.0f;
expected.x = 4.0f;
expected.y = 5.0f;
expected.z = 6.0f;
expected.w = 0.2f;
actual = quat2.mySlerp(quat1, t);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
// Input : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 1.0
// 範囲外t >= 1.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 1.0f;
expected.x = 1.0f;
expected.y = 2.0f;
expected.z = 3.0f;
expected.w = 0.1f;
actual = quat2.mySlerp(quat1, t);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
// Input : quat1{x = 0.49, y = 0.5, z = 0.5, w = 0.5}, quat2{x = 0.5, y = 0.49, z = 0.5, w = 0.5}, t = 0.5
// cosOmega <= 0.9999fのケース
quat1.x = 0.49f;
quat1.y = 0.5f;
quat1.z = 0.5f;
quat1.w = 0.5f;
quat2.x = 0.5f;
quat2.y = 0.49f;
quat2.z = 0.5f;
quat2.w = 0.5f;
t = 0.5f;
LDVector3 n0 = quat2.getAxis();
LDVector3 n1 = quat1.getAxis();
ld_float a0 = quat2.getAngle();
ld_float a1 = quat1.getAngle();
ld_float adiff = LDMathUtil.getAngleDiff(a1, a0);
ld_float at = a0 + adiff * t;
LDVector3 nt = LDVector3.blend(n0, n1, 1 - t, t);
nt.normalize();
expected.setToRotateAxis(nt, at);
actual = quat2.mySlerp(quat1, t);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
//外周の頂点インデックスを取得。yが一番小さい点から時計周りで取得。
public List <int> getOutlinePointIndices(LDPointList points)
{
List <int> result = new List <int>();
// 外周をたどる。始点に戻ったら終了。
int startIndex = math.PointUtil.findMinYPointIndex(points);
Debug.Assert(startIndex >= 0);
result.Add(startIndex);
int lastIndex = -1;
int currentIndex = startIndex;
for (int i = 0; i < points.length(); ++i)
{
//現在の頂点と接続される点一覧を取得し、その中から進行方向に対してもっとも左側に位置するものを取得
List <int> related = getRelatedPointIndices(currentIndex);
LDPoint lastPoint;
LDPoint currentPoint = points[currentIndex];
if (lastIndex == -1)
{
lastPoint = currentPoint - new LDPoint(0, 1);
}
else
{
lastPoint = points[lastIndex];
}
int nextIndex = -1;
double minAngle = 360;
foreach (var targetIndex in related)
{
LDPoint targetPoint = points[targetIndex];
if (targetIndex == lastIndex)
{
continue;
}
LDVector2 v1 = new LDVector2(lastPoint - currentPoint);
LDVector2 v2 = new LDVector2(targetPoint - currentPoint);
double angle = LDMathUtil.getAngle(v2, v1);
if (angle < minAngle)
{
minAngle = angle;
nextIndex = targetIndex;
}
}
if (nextIndex == startIndex)
{
//一周した
break;
}
result.Add(nextIndex);
lastIndex = currentIndex;
currentIndex = nextIndex;
}
return(result);
}
public void extendedTransformTest()
{
{
//拡張したGridの取得
LDGridTransform src = new LDGridTransform(20, 20, 40, 40, 3, 3);
LDGridTransform dst = src.createExtendedGrid();
TestUtil.COMPARE(dst.getRow(), 4);
TestUtil.COMPARE(dst.getColumn(), 4);
TestUtil.COMPARE(dst.getPoint(0, 0), new LDPoint(-20, -20));
TestUtil.COMPARE(dst.getPoint(0, 1), new LDPoint(10, -20));
TestUtil.COMPARE(dst.getPoint(0, 2), new LDPoint(40, -20));
TestUtil.COMPARE(dst.getPoint(0, 3), new LDPoint(70, -20));
TestUtil.COMPARE(dst.getPoint(0, 4), new LDPoint(100, -20));
TestUtil.COMPARE(dst.getPoint(1, 1), new LDPoint(20, 20));
}
{
//範囲外 順変換 クリッピング
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 1, 1);
LDPoint src = new LDPoint(-1, -1);
LDPoint dst;
dst = grid.transform(src, true);
TestUtil.COMPARE(dst.x(), 20.0);
TestUtil.COMPARE(dst.y(), 20.0);
}
{
//範囲外 順変換
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 1, 1);
LDPoint src = new LDPoint(-1, -1);
LDPoint dst;
dst = grid.transform(src);
TestUtil.COMPARE(dst.x(), -20.0);
TestUtil.COMPARE(dst.y(), -20.0);
}
{
//範囲外 順変換
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 1, 1);
LDPoint src = new LDPoint(-10, -10);
LDPoint dst;
dst = grid.transform(src);
TestUtil.COMPARE(dst.x(), -380.0);
TestUtil.COMPARE(dst.y(), -380.0);
}
{
//範囲外 逆変換 クリッピング
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 2, 2);
LDPoint src = new LDPoint(-20, -20);
LDPoint dst;
dst = grid.inverseTransform(src, true);
TestUtil.COMPARE(dst.x(), 0.0);
TestUtil.COMPARE(dst.y(), 0.0);
}
{
//範囲外 逆変換
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 2, 2);
LDPoint src = new LDPoint(-20, -20);
LDPoint dst;
dst = grid.inverseTransform(src);
TestUtil.COMPARE(dst.x(), -1.0);
TestUtil.COMPARE(dst.y(), -1.0);
}
{
LDGridTransform grid = new LDGridTransform(
new LDPoint(20.53125f, 20.62423f)
, new LDPoint(40.53125f, 20.62423f)
, new LDPoint(45.53125f, 45.62423f)
, new LDPoint(20.614312f, 40.94645f), 2, 2);
LDGridTransform src = new LDGridTransform(3425.0134623f, 2412.9143f, 5252 - 2412.090023f, 5212 - 2451.00001f, 2, 8);
LDGridTransform dst = src;
var points = grid.inverseTransform(src.toForm());
var rest = grid.transform(points);
dst.setForm(points);
TestUtil.VERIFY(LDMathUtil.fuzzyCompare(rest, src.toForm(), 0.0000001f));
}
{
LDGridTransform grid = new LDGridTransform(
new LDPoint(2012.53125f, 2051.62423f)
, new LDPoint(4097.53125f, 2033.62423f)
, new LDPoint(4575.53125f, 4566.62423f)
, new LDPoint(2062.614312f, 4000.94645f), 2, 2);
LDGridTransform src = new LDGridTransform(34.0134623f, 24.9143f, 52 - 24.090023f, 52 - 24.00001f, 8, 2);
LDGridTransform dst = src;
var points = grid.inverseTransform(src.toForm());
var rest = grid.transform(points);
dst.setForm(points);
TestUtil.VERIFY(LDMathUtil.fuzzyCompare(rest, src.toForm(), 0.0000001f));
}
}
public void inverseTransformTest()
{
{
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 2, 2);
LDPoint src = new LDPoint(30, 30);
LDPoint dst = new LDPoint();
dst = grid.inverseTransform(src);
TestUtil.COMPARE(dst.x(), 0.25);
TestUtil.COMPARE(dst.y(), 0.25);
}
{
LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 2, 2);
LDPointList src = new LDPointList();
src.add(new LDPoint(30, 30)).add(new LDPoint(50, 50));
LDPointList dst;
dst = grid.inverseTransform(src);
TestUtil.COMPARE(dst[0], new LDPoint(0.25f, 0.25f));
TestUtil.COMPARE(dst[1], new LDPoint(0.75f, 0.75f));
}
/*
* {
* LDGridTransform grid = new LDGridTransform(20, 20, 40, 40, 2, 2);
* LDGridTransform src = new LDGridTransform(24, 24, 52 - 24, 52 - 24, 2, 2);
*
* LDGridTransform dst = src;
*
* var points = grid.inverseTransform(src.toForm());
*
*
* dst.setForm(points);
* LDFUZZY_COMPARE(dst.getPoint(0, 0).x(), 0.1f, 0.0000001f);
* LDFUZZY_COMPARE(dst.getPoint(2, 2).x(), 0.8f, 0.0000001f);
* LDFUZZY_COMPARE(dst.getPoint(2, 0).x(), 0.1f, 0.0000001f);
* LDFUZZY_COMPARE(dst.getPoint(2, 0).y(), 0.8f, 0.0000001f);
* }
*/
{
LDGridTransform grid = new LDGridTransform(20.53125f, 20.62423f, 40.614312f, 40.94645f, 2, 2);
LDGridTransform src = new LDGridTransform(24.0134623f, 24.9143f, 52 - 24.090023f, 52 - 24.00001f, 2, 2);
LDGridTransform dst = new LDGridTransform(src);
var points = grid.inverseTransform(src.toForm());
var rest = grid.transform(points);
dst.setForm(points);
TestUtil.VERIFY(LDMathUtil.fuzzyCompare(rest, src.toForm(), 0.0000001f));
}
{
LDGridTransform grid = new LDGridTransform(2530.53125f, 2540.62423f, 4015.614312f, 4026.94645f, 2, 2);
LDGridTransform src = new LDGridTransform(2594.0134623f, 2594.9143f, 5274 - 2594.090023f, 5276 - 2594.00001f, 2, 2);
LDGridTransform dst = new LDGridTransform(src);
var points = grid.inverseTransform(src.toForm());
var rest = grid.transform(points);
dst.setForm(points);
TestUtil.VERIFY(LDMathUtil.fuzzyCompare(rest, src.toForm(), 0.0000001f));
}
{
LDGridTransform grid = new LDGridTransform(
new LDPoint(20.53125f, 20.62423f)
, new LDPoint(40.53125f, 20.62423f)
, new LDPoint(45.53125f, 45.62423f)
, new LDPoint(20.614312f, 40.94645f), 2, 2);
LDGridTransform src = new LDGridTransform(34.0134623f, 24.9143f, 52 - 24.090023f, 52 - 24.00001f, 8, 8);
LDGridTransform dst = new LDGridTransform(src);
var points = grid.inverseTransform(src.toForm());
var rest = grid.transform(points);
dst.setForm(points);
TestUtil.VERIFY(LDMathUtil.fuzzyCompare(rest, src.toForm(), 0.0000001f));
}
{
LDGridTransform grid = new LDGridTransform(
new LDPoint(2012.53125f, 2051.62423f)
, new LDPoint(4097.53125f, 2033.62423f)
, new LDPoint(4575.53125f, 4566.62423f)
, new LDPoint(2062.614312f, 4000.94645f), 2, 2);
LDGridTransform src = new LDGridTransform(3444.0134623f, 2442.9143f, 5242 - 2412.090023f, 5211 - 2467.00001f, 8, 8);
LDGridTransform dst = new LDGridTransform(src);
var points = grid.inverseTransform(src.toForm());
var rest = grid.transform(points);
dst.setForm(points);
TestUtil.VERIFY(LDMathUtil.fuzzyCompare(rest, src.toForm(), 0.0000001f));
}
}
