| 1 |
#include "stdafx.h" |
| 2 |
#include "CRailCurve.h" |
| 3 |
#include "CRailTraceCurve.h" |
| 4 |
|
| 5 |
// 内部定数 |
| 6 |
extern const float RAIL_SEG_MAX = 100.0f; // レール分割最大値 |
| 7 |
|
| 8 |
/* |
| 9 |
* 分割点計算 |
| 10 |
*/ |
| 11 |
bool CRailCurve::CalcSplit( |
| 12 |
VEC3 &pos1, VEC3 &dir1, // 始点 |
| 13 |
VEC3 &pos2, VEC3 &dir2 // 終点 |
| 14 |
){ |
| 15 |
m_Radius = -1.0f; |
| 16 |
m_CompSplit = false; |
| 17 |
m_SplitDir = pos2-pos1; |
| 18 |
m_SegLen = V3Len(&m_SplitDir); |
| 19 |
V3Norm(&m_SplitDir, &m_SplitDir); |
| 20 |
if(V3Len(&(pos1+m_SegLen*dir1-pos2))+V3Len(&(pos1+m_SegLen*dir2-pos2))<0.2f){ |
| 21 |
m_SplitPos = 0.5f*(pos1+pos2); |
| 22 |
return m_SegLen<=RAIL_SEG_MAX; |
| 23 |
} |
| 24 |
VEC3 m1, m2, cctmp, to1, to2; |
| 25 |
float dot1, dot2, tl1, tl2, dif; |
| 26 |
if(LineLineNearest(&m1, &m2, &pos1, &dir1, &pos2, &dir2)){ |
| 27 |
// parallel |
| 28 |
if(V3Dot(&dir1, &dir2)>0.001f){ |
| 29 |
cctmp = 0.5f*(pos1+pos2); to1 = cctmp-pos1; to2 = cctmp-pos2; |
| 30 |
dot1 = V3Dot(&to1, &dir1); dot2 = V3Dot(&to2, &dir2); |
| 31 |
if(dot1*dot2<=0.0f){ |
| 32 |
m_CompSplit = true; |
| 33 |
m_SplitPos = 0.5f*(pos1+pos2); |
| 34 |
V3Norm(&cctmp, &(dir1+dir2)); |
| 35 |
V3Norm(&m_SplitDir, &(-cctmp+2.0f*m_SplitDir*V3Dot(&cctmp, &m_SplitDir))); |
| 36 |
return false; |
| 37 |
} |
| 38 |
}else{ |
| 39 |
dif = V3Dot(&dir1, &(pos2-pos1)); |
| 40 |
goto LINECOMP; |
| 41 |
} |
| 42 |
}else{ |
| 43 |
cctmp = 0.5f*(m1+m2); to1 = cctmp-pos1; to2 = cctmp-pos2; |
| 44 |
dot1 = V3Dot(&to1, &dir1); dot2 = V3Dot(&to2, &dir2); |
| 45 |
tl1 = V3Len(&to1); tl2 = V3Len(&to2); dif = tl1-tl2; |
| 46 |
if(tl1<0.001f || tl2<0.001f || dot1*dot2>=0.0f){ |
| 47 |
m_CompSplit = true; |
| 48 |
dot1 = V3Dot(&m_SplitDir, &dir1); dot2 = V3Dot(&m_SplitDir, &dir2); |
| 49 |
if(fabsf(dot1)<fabsf(dot2)){ |
| 50 |
V3Norm(&to1, &(m_SplitDir-dir1*dot1)); |
| 51 |
V3Norm(&to2, &(dir1-dir2*V3Dot(&dir1, &dir2))); |
| 52 |
if(V3Dot(&dir2, &m_SplitDir)<0.0f) to2 = -to2; |
| 53 |
}else{ |
| 54 |
V3Norm(&to2, &(dir2*dot2-m_SplitDir)); |
| 55 |
V3Norm(&to1, &(dir1*V3Dot(&dir1, &dir2)-dir2)); |
| 56 |
if(V3Dot(&to1, &m_SplitDir)<0.0f) to1 = -to1; |
| 57 |
} |
| 58 |
float r = 0.25f*m_SegLen, lbound = r, ubound = r; |
| 59 |
while(true){ |
| 60 |
float dif = V3Len(&((pos1+to1*r)-(pos2+to2*r)))-2.0f*r; |
| 61 |
if(fabsf(dif)<0.001f) break; |
| 62 |
if(dif>0.0f){ |
| 63 |
if(r==ubound) ubound = r *= 2.0f; |
| 64 |
else r = 0.5f*((lbound = r)+ubound); |
| 65 |
}else{ |
| 66 |
r = 0.5f*((ubound = r)+lbound); |
| 67 |
} |
| 68 |
} |
| 69 |
m_SplitPos = 0.5f*((m1 = pos1+to1*r)+(m2 = pos2+to2*r)); |
| 70 |
VEC3 u1, u2, s1, s2; |
| 71 |
VEC3 out1 = m_SplitPos-m1, out2 = m_SplitPos-m2; |
| 72 |
V3Norm(&s1, V3Cross(&s1, V3Cross(&u1, &dir1, &to1), &out1)); |
| 73 |
V3Norm(&s2, V3Cross(&s2, V3Cross(&u2, &dir2, &to2), &out2)); |
| 74 |
V3Norm(&m_SplitDir, &(s1+s2)); |
| 75 |
return false; |
| 76 |
} |
| 77 |
if(dot1<0.0f) dif = -dif; |
| 78 |
LINECOMP:; |
| 79 |
if(dif>1.0f){ |
| 80 |
m_CompSplit = true; |
| 81 |
m_SplitPos = pos1+dif*dir1; |
| 82 |
m_SplitDir = dir1; |
| 83 |
return false; |
| 84 |
} |
| 85 |
if(dif<-1.0f){ |
| 86 |
m_CompSplit = true; |
| 87 |
m_SplitPos = pos2+dif*dir2; |
| 88 |
m_SplitDir = dir2; |
| 89 |
return false; |
| 90 |
} |
| 91 |
} |
| 92 |
VEC3 mid = 0.5f*(pos1+pos2); |
| 93 |
float theta = acosf(V3Dot(&dir1, &dir2)); |
| 94 |
m_Radius = 0.5f*m_SegLen/sinf(0.5f*theta); |
| 95 |
VEC3 out = dir1-dir2; |
| 96 |
V3Norm(&out, &out); |
| 97 |
m_SplitDir = pos2-pos1; |
| 98 |
if(V3Dot(&m_SplitDir, &dir2)<0.0f){ |
| 99 |
m_SplitPos = (1.0f+cosf(0.5f*theta))*m_Radius*out+mid; |
| 100 |
V3Norm(&out, &(-dir1-dir2)); |
| 101 |
VEC3 vert = m_SplitDir-out*V3Dot(&out, &m_SplitDir); |
| 102 |
m_SplitDir = out*(m_Radius*(2.0f*D3DX_PI-theta))+2.0f*vert; |
| 103 |
}else{ |
| 104 |
m_SplitPos = (1.0f-cosf(0.5f*theta))*m_Radius*out+mid; |
| 105 |
V3Norm(&out, &(dir1+dir2)); |
| 106 |
VEC3 vert = m_SplitDir-out*V3Dot(&out, &m_SplitDir); |
| 107 |
m_SplitDir = out*(m_Radius*theta)+2.0f*vert; |
| 108 |
} |
| 109 |
V3Norm(&m_SplitDir, &m_SplitDir); |
| 110 |
return false; |
| 111 |
} |
| 112 |
|
| 113 |
/* |
| 114 |
* 半径計算 |
| 115 |
*/ |
| 116 |
bool CRailCurve::CalcRadius( |
| 117 |
VEC3 &pos1, VEC3 &dir1, // 始点 |
| 118 |
VEC3 &pos2, VEC3 &dir2 // 終点 |
| 119 |
){ |
| 120 |
if(dir1==dir2) return false; |
| 121 |
float theta = acosf(V3Dot(&dir1, &dir2)); |
| 122 |
m_Radius = 0.5f*m_SegLen/sinf(0.5f*theta); |
| 123 |
return true; |
| 124 |
} |
| 125 |
|
| 126 |
/* |
| 127 |
* 曲線長さ計算 |
| 128 |
*/ |
| 129 |
float CRailCurve::CalcLength( |
| 130 |
VEC3 &pos1, VEC3 &dir1, // 始点 |
| 131 |
VEC3 &pos2, VEC3 &dir2, // 終点 |
| 132 |
float sum // 累計長さ |
| 133 |
){ |
| 134 |
if(CalcSplit(pos1, dir1, pos2, dir2)) return sum+V3Len(&(pos2-pos1)); |
| 135 |
CRailCurve curve; |
| 136 |
return curve.CalcLength(m_SplitPos, m_SplitDir, pos2, dir2, |
| 137 |
curve.CalcLength(pos1, dir1, m_SplitPos, m_SplitDir, sum)); |
| 138 |
} |
| 139 |
|
| 140 |
//////////////////////////////////////////////////////////////////////////////// |
| 141 |
//////////////////////////////////////////////////////////////////////////////// |
| 142 |
|
| 143 |
// static メンバ |
| 144 |
bool CRailTraceCurve::ms_Terminate1; |
| 145 |
bool CRailTraceCurve::ms_Terminate2; |
| 146 |
IRailSplitter CRailTraceCurve::ms_SpliceItr; |
| 147 |
|
| 148 |
//////////////////////////////////////////////////////////////////////////////// |
| 149 |
//////////////////////////////////////////////////////////////////////////////// |
| 150 |
|
| 151 |
/* |
| 152 |
* カントから座標系計算 |
| 153 |
*/ |
| 154 |
void CalcCantAxis( |
| 155 |
VEC3 *right, VEC3 *up, VEC3 *dir, // 代入先 |
| 156 |
float cant // カント量 |
| 157 |
){ |
| 158 |
*up = V3UP; |
| 159 |
V3NormAxis(right, up, dir); |
| 160 |
*up += *right*cant; |
| 161 |
V3NormAxis(right, up, dir); |
| 162 |
} |
RailSim II
Parent Directory
Revision Log