/// 点の移動と三角形との衝突チェック
/**
* moveLineの軌道による点の移動とtrgTriとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
static public bool CheckLineAndTriangle(GeometryLine moveLine, GeometryTriangle trgTri, ref Vector3 collPos)
{
/// 表裏判定
if (moveLine.Vec.Dot(trgTri.Plane.Nor) >= 0.0f)
{
return(false);
}
/// 平面との交差チェック
if (checkRayCrossPlane(trgTri.Plane, moveLine) == false)
{
return(false);
}
/// 平面との交点を求める
if (getRayPlaneCrossPoint(trgTri.Plane, moveLine, ref collPos) < epsilon)
{
return(false);
}
/// 三角形の内外判定
if (checkInsideTriangle(trgTri, collPos) == false)
{
return(false);
}
return(true);
}
/// 線分と平面との交点をもとめる
static private float getRayPlaneCrossPoint(GeometryPlane plane, GeometryLine line, ref Vector3 crossPos)
{
float num, denom;
float t;
/// 法線とレイの１頂点とのベクトルの内積を求める
num = plane.Nor.Dot(line.StartPos);
/// 法線とレイの内積を求める
denom = plane.Nor.Dot(line.Vec);
/// 平面と平行なので交点なし
if (denom == 0.0f)
{
return(-1.0f);
}
/// 媒介変数を求める
t = (-(num + plane.D)) / denom;
/// 直線の方程式から交点を求める
crossPos.X = line.StartPos.X + (t * line.Vec.X);
crossPos.Y = line.StartPos.Y + (t * line.Vec.Y);
crossPos.Z = line.StartPos.Z + (t * line.Vec.Z);
return(t);
}
/// Line描画
public void DrawLine(GraphicsContext graphics, GeometryLine trgLine, Camera cam, Rgba color)
{
if (debShader == null)
{
return;
}
/// バーテクスの更新
///------------------------------------------------------------
for (int i = 0; i < 2; i++)
{
debMesh.Positions[i * 3 + 0] = trgLine.GetPos(i).X;
debMesh.Positions[i * 3 + 1] = trgLine.GetPos(i).Y;
debMesh.Positions[i * 3 + 2] = trgLine.GetPos(i).Z;
debMesh.Normals[i * 3 + 0] = 0.0f;
debMesh.Normals[i * 3 + 1] = 1.0f;
debMesh.Normals[i * 3 + 2] = 0.0f;
}
debVb.SetVertices(0, debMesh.Positions);
debVb.SetIndices(debMesh.Indices);
drawMesh(graphics, cam, color);
}
/// 線分と平面との交差ﾁｪｯｸ
static private bool checkRayCrossPlane(GeometryPlane plane, GeometryLine line)
{
float sign1, sign2;
/// 法線と頂点との位置関係を内積で求める（正：面の正面、負：面の背後）
sign1 = plane.Nor.Dot(line.StartPos) + plane.D;
sign2 = plane.Nor.Dot(line.EndPos) + plane.D;
/// 符合が一致すると平面に対して交差した事にならない
if ((sign1 > epsilon && sign2 > epsilon) || (sign1 < epsilon && sign2 < epsilon))
{
return(false);
}
return(true);
}
/// public メンバ （点の移動）
///---------------------------------------------------------------------------
/// 点の移動とカプセルとの衝突チェック
/**
* moveLineの軌道での点の移動とtrgCapとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
static public bool CheckLineAndCapsule(GeometryLine moveLine, GeometryCapsule trgCap, ref Vector3 collPos)
{
Vector3 c1 = new Vector3(0, 0, 0);
Vector3 c2 = new Vector3(0, 0, 0);
float dist = getClosestPtLineLine(moveLine, trgCap.Line, ref c1, ref c2);
float radius = trgCap.R;
/// カプセル同士の衝突は無し
if (dist > radius * radius)
{
return(false);
}
/// カプセル同士の衝突点の算出
///-----------------------------------------------
/// カプセルの中心の線分同士が交わる
if (dist <= epsilon)
{
/// 線分同士の交点を用いて、２つの線分の角度を求める
float rad = getRadian(c1, moveLine.StartPos, trgCap.Line.EndPos);
/// 衝突点を求める
float sin = FMath.Sin(rad);
if (sin > 0.0f)
{
float dis = radius / sin;
collPos = (moveLine.Vec * dis * -1) + c1;
}
else
{
collPos = (moveLine.Vec * radius * -1) + c1;
}
}
/// カプセルの外側の球に接触する
else
{
/// 対象カプセルの中心線分の最近接点に球体を生成、その球体と移動する球の軌道との衝突を行い衝突点を算出する
calSph.Set(c2, trgCap.R);
CheckLineAndSphere(moveLine, calSph, ref collPos);
}
return(true);
}
/// 点の移動と球との衝突チェック
/**
* moveLineの軌道による点の移動とtrgSphとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
static public bool CheckLineAndSphere(GeometryLine moveLine, GeometrySphere trgSph, ref Vector3 collPos)
{
Vector3 calVec = trgSph.Pos - moveLine.StartPos;
calVec = calVec.Normalize();
if (calVec.Dot(moveLine.Vec) >= 0.0f)
{
Vector3 calVec2 = trgSph.Pos - moveLine.StartPos;
float isT = FMath.Sqrt(calVec2.Dot(calVec2));
if (isT < (moveLine.Length + trgSph.R))
{
float dis = checkRayCrossSphere(moveLine.StartPos, moveLine.Vec, trgSph);
if (dis >= 0.0f || dis < moveLine.Length)
{
collPos = moveLine.StartPos + (moveLine.Vec * dis);
return(true);
}
}
}
return(false);
}
public GeometryCapsule( Vector3 sPos, Vector3 ePos, float radius )
{
this.Line = new GeometryLine();
Set( sPos, ePos, radius );
}
/// ２つの線分の最近接点の算出
static private float getClosestPtLineLine(GeometryLine p1, GeometryLine p2, ref Vector3 c1, ref Vector3 c2)
{
Vector3 d1 = p1.EndPos - p1.StartPos;
Vector3 d2 = p2.EndPos - p2.StartPos;
Vector3 r = p1.StartPos - p2.StartPos;
float a = d1.Dot(d1);
float e = d2.Dot(d2);
float f = d2.Dot(r);
Vector3 cal;
/// 両方とも線分が点になっている
if (a <= epsilon && e <= epsilon)
{
length1 = length2 = 0.0f;
c1 = p1.StartPos;
c2 = p2.StartPos;
cal = c1 - c2;
return(cal.Dot(cal));
}
/// 最初の線分が点に縮退
else if (a <= epsilon)
{
length1 = 0.0f;
length2 = f / e;
length2 = FMath.Clamp(length2, 0.0f, 1.0f);
}
else
{
float c = d1.Dot(r);
/// ２番目の線分が点に縮退
if (e <= epsilon)
{
length2 = 0.0f;
length1 = FMath.Clamp(-c / a, 0.0f, 1.0f);
}
else
{
float b = d1.Dot(d2);
float denom = a * e - b * b;
/// ２つの線分が平行
if (denom == 0.0f)
{
length1 = 0.0f;
}
else
{
length1 = FMath.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
}
length2 = (b * length1 + f) / e;
if (length2 < 0.0f)
{
length2 = 0.0f;
length1 = FMath.Clamp(-c / a, 0.0f, 1.0f);
}
else if (length2 > 1.0f)
{
length2 = 1.0f;
length1 = FMath.Clamp((b - c) / a, 0.0f, 1.0f);
}
}
}
c1 = p1.StartPos + d1 * length1;
c2 = p2.StartPos + d2 * length2;
cal = c1 - c2;
return(cal.Dot(cal));
}
/// 点の移動と三角形との衝突チェック
/**
* moveLineの軌道による点の移動とtrgTriとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
public static bool CheckLineAndTriangle( GeometryLine moveLine, GeometryTriangle trgTri, ref Vector3 collPos )
{
/// 表裏判定
if( moveLine.Vec.Dot( trgTri.Plane.Nor ) >= 0.0f ){
return false;
}
/// 平面との交差チェック
if( checkRayCrossPlane( trgTri.Plane, moveLine ) == false ){
return false;
}
/// 平面との交点を求める
if( getRayPlaneCrossPoint( trgTri.Plane, moveLine, ref collPos ) < epsilon ){
return false;
}
/// 三角形の内外判定
if( checkInsideTriangle( trgTri, collPos ) == false ){
return false;
}
return true;
}
/// public メンバ （最近接点の算出）
///---------------------------------------------------------------------------
/// 点と線分の最近接点を算出
static public float GetClosestPtPosLine(Vector3 trgPos, GeometryLine trgLine, ref Vector3 cPos)
{
return(getClosestPtPosLine(trgPos, trgLine.StartPos, trgLine.EndPos, ref cPos));
}
public GeometryCapsule(Vector3 sPos, Vector3 ePos, float radius)
{
this.Line = new GeometryLine();
Set(sPos, ePos, radius);
}
/// public メンバ （最近接点の算出）
///---------------------------------------------------------------------------
/// 点と線分の最近接点を算出
public static float GetClosestPtPosLine( Vector3 trgPos, GeometryLine trgLine, ref Vector3 cPos )
{
return( getClosestPtPosLine( trgPos, trgLine.StartPos, trgLine.EndPos, ref cPos ) );
}
/// 点の移動と球との衝突チェック
/**
* moveLineの軌道による点の移動とtrgSphとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
public static bool CheckLineAndSphere( GeometryLine moveLine, GeometrySphere trgSph, ref Vector3 collPos )
{
Vector3 calVec = trgSph.Pos - moveLine.StartPos;
calVec = calVec.Normalize();
if( calVec.Dot( moveLine.Vec ) >= 0.0f ){
Vector3 calVec2 = trgSph.Pos - moveLine.StartPos;
float isT = FMath.Sqrt( calVec2.Dot(calVec2) );
if( isT < (moveLine.Length+trgSph.R) ){
float dis = checkRayCrossSphere( moveLine.StartPos, moveLine.Vec, trgSph );
if( dis >= 0.0f || dis < moveLine.Length ){
collPos = moveLine.StartPos + (moveLine.Vec * dis);
return true;
}
}
}
return false;
}
/// コンストラクタ
public GeometryCapsule()
{
this.Line = new GeometryLine();
}
/// public メンバ （点の移動）
///---------------------------------------------------------------------------
/// 点の移動とカプセルとの衝突チェック
/**
* moveLineの軌道での点の移動とtrgCapとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
public static bool CheckLineAndCapsule( GeometryLine moveLine, GeometryCapsule trgCap, ref Vector3 collPos )
{
Vector3 c1 = new Vector3(0,0,0);
Vector3 c2 = new Vector3(0,0,0);
float dist = getClosestPtLineLine( moveLine, trgCap.Line, ref c1, ref c2 );
float radius = trgCap.R;
/// カプセル同士の衝突は無し
if( dist > radius*radius ){
return false;
}
/// カプセル同士の衝突点の算出
///-----------------------------------------------
/// カプセルの中心の線分同士が交わる
if( dist <= epsilon ){
/// 線分同士の交点を用いて、２つの線分の角度を求める
float rad = getRadian( c1, moveLine.StartPos, trgCap.Line.EndPos );
/// 衝突点を求める
float sin = FMath.Sin( rad );
if( sin > 0.0f ){
float dis = radius / sin;
collPos = (moveLine.Vec * dis * -1) + c1;
}
else{
collPos = (moveLine.Vec * radius * -1) + c1;
}
}
/// カプセルの外側の球に接触する
else{
/// 対象カプセルの中心線分の最近接点に球体を生成、その球体と移動する球の軌道との衝突を行い衝突点を算出する
calSph.Set( c2, trgCap.R );
CheckLineAndSphere( moveLine, calSph, ref collPos );
}
return true;
}
/// 線分と平面との交点をもとめる
private static float getRayPlaneCrossPoint( GeometryPlane plane, GeometryLine line, ref Vector3 crossPos )
{
float num, denom;
float t;
/// 法線とレイの１頂点とのベクトルの内積を求める
num = plane.Nor.Dot( line.StartPos );
/// 法線とレイの内積を求める
denom = plane.Nor.Dot( line.Vec );
/// 平面と平行なので交点なし
if( denom == 0.0f ){
return -1.0f;
}
/// 媒介変数を求める
t = ( -(num + plane.D) ) / denom;
/// 直線の方程式から交点を求める
crossPos.X = line.StartPos.X + ( t * line.Vec.X );
crossPos.Y = line.StartPos.Y + ( t * line.Vec.Y );
crossPos.Z = line.StartPos.Z + ( t * line.Vec.Z );
return t;
}
/// ２つの線分の最近接点の算出
private static float getClosestPtLineLine( GeometryLine p1, GeometryLine p2, ref Vector3 c1, ref Vector3 c2 )
{
Vector3 d1 = p1.EndPos - p1.StartPos;
Vector3 d2 = p2.EndPos - p2.StartPos;
Vector3 r = p1.StartPos - p2.StartPos;
float a = d1.Dot(d1);
float e = d2.Dot(d2);
float f = d2.Dot(r);
Vector3 cal;
/// 両方とも線分が点になっている
if( a <= epsilon && e <= epsilon ){
length1 = length2 = 0.0f;
c1 = p1.StartPos;
c2 = p2.StartPos;
cal = c1 - c2;
return( cal.Dot(cal) );
}
/// 最初の線分が点に縮退
else if( a <= epsilon ){
length1 = 0.0f;
length2 = f / e;
length2 = FMath.Clamp( length2, 0.0f, 1.0f );
}
else{
float c = d1.Dot( r );
/// ２番目の線分が点に縮退
if( e <= epsilon ){
length2 = 0.0f;
length1 = FMath.Clamp( -c/a, 0.0f, 1.0f );
}
else{
float b = d1.Dot( d2 );
float denom = a*e - b*b;
/// ２つの線分が平行
if( denom == 0.0f ){
length1 = 0.0f;
}
else{
length1 = FMath.Clamp( (b*f - c*e) / denom, 0.0f, 1.0f );
}
length2 = (b*length1 + f) / e;
if( length2 < 0.0f ){
length2 = 0.0f;
length1 = FMath.Clamp( -c/a, 0.0f, 1.0f );
}
else if( length2 > 1.0f ){
length2 = 1.0f;
length1 = FMath.Clamp( (b-c)/a, 0.0f, 1.0f );
}
}
}
c1 = p1.StartPos + d1 * length1;
c2 = p2.StartPos + d2 * length2;
cal = c1 - c2;
return( cal.Dot( cal ) );
}
/// 線分と平面との交差ﾁｪｯｸ
private static bool checkRayCrossPlane( GeometryPlane plane, GeometryLine line )
{
float sign1, sign2;
/// 法線と頂点との位置関係を内積で求める（正：面の正面、負：面の背後）
sign1 = plane.Nor.Dot( line.StartPos ) + plane.D;
sign2 = plane.Nor.Dot( line.EndPos ) + plane.D;
/// 符合が一致すると平面に対して交差した事にならない
if( (sign1 > epsilon && sign2 > epsilon) || (sign1 < epsilon && sign2 < epsilon) ){
return false;
}
return true;
}
/// コンストラクタ
public GeometryCapsule()
{
this.Line = new GeometryLine();
}
/// Line描画
public void DrawLine( GraphicsContext graphics, GeometryLine trgLine, Camera cam, Rgba color )
{
if( debShader == null ){
return ;
}
/// バーテクスの更新
///------------------------------------------------------------
for( int i=0; i<2; i++ ){
debMesh.Positions[ i*3+0 ] = trgLine.GetPos(i).X;
debMesh.Positions[ i*3+1 ] = trgLine.GetPos(i).Y;
debMesh.Positions[ i*3+2 ] = trgLine.GetPos(i).Z;
debMesh.Normals[ i*3+0 ] = 0.0f;
debMesh.Normals[ i*3+1 ] = 1.0f;
debMesh.Normals[ i*3+2 ] = 0.0f;
}
debVb.SetVertices( 0, debMesh.Positions );
debVb.SetIndices( debMesh.Indices );
drawMesh( graphics, cam, color );
}
