/// 三角形描画
public void DrawTriangle(GraphicsContext graphics, GeometryTriangle trgTri, Camera cam, Rgba color)
{
if (debShader == null)
{
return;
}
/// バーテクスの更新
///------------------------------------------------------------
for (int i = 0; i < 3; i++)
{
debMesh.Positions[i * 3 + 0] = trgTri.GetPos(i).X;
debMesh.Positions[i * 3 + 1] = trgTri.GetPos(i).Y;
debMesh.Positions[i * 3 + 2] = trgTri.GetPos(i).Z;
debMesh.Normals[i * 3 + 0] = trgTri.Plane.Nor.X;
debMesh.Normals[i * 3 + 1] = trgTri.Plane.Nor.Y;
debMesh.Normals[i * 3 + 2] = trgTri.Plane.Nor.Z;
}
debVb.SetVertices(0, debMesh.Positions);
debVb.SetIndices(debMesh.Indices);
drawMesh(graphics, cam, color);
}
/// 三角形と点との内外判定
static private bool checkInsideTriangle(GeometryTriangle trgTri, Vector3 crossPos)
{
int type;
float d1, d2, d3;
if (FMath.Abs(trgTri.Plane.Nor.X) < FMath.Abs(trgTri.Plane.Nor.Y))
{
type = (FMath.Abs(trgTri.Plane.Nor.Y) < FMath.Abs(trgTri.Plane.Nor.Z)) ? 3 : 2;
}
else
{
type = (FMath.Abs(trgTri.Plane.Nor.X) < FMath.Abs(trgTri.Plane.Nor.Z)) ? 3 : 1;
}
switch (type)
{
/// X方向に面が傾いているためX軸を破棄
case 1:
d1 = ((crossPos.Y - trgTri.Pos1.Y) * (crossPos.Z - trgTri.Pos2.Z)) -
((crossPos.Z - trgTri.Pos1.Z) * (crossPos.Y - trgTri.Pos2.Y));
d2 = ((crossPos.Y - trgTri.Pos2.Y) * (crossPos.Z - trgTri.Pos3.Z)) -
((crossPos.Z - trgTri.Pos2.Z) * (crossPos.Y - trgTri.Pos3.Y));
d3 = ((crossPos.Y - trgTri.Pos3.Y) * (crossPos.Z - trgTri.Pos1.Z)) -
((crossPos.Z - trgTri.Pos3.Z) * (crossPos.Y - trgTri.Pos1.Y));
break;
/// y方向に面が傾いているためY軸を破棄
case 2:
d1 = ((crossPos.X - trgTri.Pos1.X) * (crossPos.Z - trgTri.Pos2.Z)) -
((crossPos.Z - trgTri.Pos1.Z) * (crossPos.X - trgTri.Pos2.X));
d2 = ((crossPos.X - trgTri.Pos2.X) * (crossPos.Z - trgTri.Pos3.Z)) -
((crossPos.Z - trgTri.Pos2.Z) * (crossPos.X - trgTri.Pos3.X));
d3 = ((crossPos.X - trgTri.Pos3.X) * (crossPos.Z - trgTri.Pos1.Z)) -
((crossPos.Z - trgTri.Pos3.Z) * (crossPos.X - trgTri.Pos1.X));
break;
/// Z方向に面が傾いているためZ軸を破棄
case 3:
d1 = ((crossPos.X - trgTri.Pos1.X) * (crossPos.Y - trgTri.Pos2.Y)) -
((crossPos.Y - trgTri.Pos1.Y) * (crossPos.X - trgTri.Pos2.X));
d2 = ((crossPos.X - trgTri.Pos2.X) * (crossPos.Y - trgTri.Pos3.Y)) -
((crossPos.Y - trgTri.Pos2.Y) * (crossPos.X - trgTri.Pos3.X));
d3 = ((crossPos.X - trgTri.Pos3.X) * (crossPos.Y - trgTri.Pos1.Y)) -
((crossPos.Y - trgTri.Pos3.Y) * (crossPos.X - trgTri.Pos1.X));
break;
default:
return(true);
}
/// 全て符号が同じなら当り。
if ((d1 >= -epsilon && d2 >= -epsilon && d3 >= -epsilon) ||
(d1 <= epsilon && d2 <= epsilon && d3 <= epsilon))
{
return(true);
}
return(false);
}
/// 点の移動と三角形との衝突チェック
/**
* 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 getClosestPtPosTriangle(Vector3 trgPos, GeometryTriangle trgTri, ref Vector3 cPos)
{
float isDis, bestDis;
Vector3 crosePos = new Vector3(0, 0, 0);
bestDis = 0.0f;
for (int i = 0; i < 3; i++)
{
isDis = getClosestPtPosLine(trgPos, trgTri.GetPos(i), trgTri.GetPos((i + 1) % 3), ref crosePos);
/// 最近接点を更新
if (i == 0 || isDis < bestDis)
{
bestDis = isDis;
cPos = crosePos;
}
}
return(bestDis);
}
/// public メンバ （球の移動）
///---------------------------------------------------------------------------
/// 球の移動と三角形との衝突チェック
/**
* moveCapの軌道による球の移動とtrgTriとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
static public bool CheckSphereAndTriangle(GeometryCapsule moveCap, GeometryTriangle trgTri, ref Vector3 collPos)
{
/// 表裏判定
if (moveCap.Line.Vec.Dot(trgTri.Plane.Nor) >= 0.0f)
{
return(false);
}
Vector3 firstHitPos, firstHitMovePos;
float trgDis;
bool isCrossFlg;
/// 三角形の平面との球の最近接点（頂点P）を算出
firstHitPos.X = moveCap.Line.StartPos.X + (moveCap.R * (trgTri.Plane.Nor.X * -1));
firstHitPos.Y = moveCap.Line.StartPos.Y + (moveCap.R * (trgTri.Plane.Nor.Y * -1));
firstHitPos.Z = moveCap.Line.StartPos.Z + (moveCap.R * (trgTri.Plane.Nor.Z * -1));
/// 既にPと球の中心が三角形を跨いでいるかチェック
isCrossFlg = false;
/// 符合が一致すると平面に対して交差した事にならない
float sign1, sign2;
sign1 = trgTri.Plane.Nor.Dot(moveCap.Line.StartPos) + trgTri.Plane.D;
sign2 = trgTri.Plane.Nor.Dot(firstHitPos) + trgTri.Plane.D;
if ((sign1 > epsilon && sign2 > epsilon) || (sign1 < epsilon && sign2 < epsilon))
{
calLine.Vec = moveCap.Line.Vec;
calLine.Length = moveCap.Line.Length;
calLine.StartPos = firstHitPos;
/// Pが移動する地点P'を算出
calLine.EndPos.X = firstHitPos.X + (moveCap.Line.Vec.X * moveCap.Line.Length);
calLine.EndPos.Y = firstHitPos.Y + (moveCap.Line.Vec.Y * moveCap.Line.Length);
calLine.EndPos.Z = firstHitPos.Z + (moveCap.Line.Vec.Z * moveCap.Line.Length);
}
else
{
isCrossFlg = true;
/// Pが移動する地点P'を算出
firstHitMovePos.X = moveCap.Line.StartPos.X + (moveCap.Line.Vec.X * (moveCap.Line.Length + moveCap.R));
firstHitMovePos.Y = moveCap.Line.StartPos.Y + (moveCap.Line.Vec.Y * (moveCap.Line.Length + moveCap.R));
firstHitMovePos.Z = moveCap.Line.StartPos.Z + (moveCap.Line.Vec.Z * (moveCap.Line.Length + moveCap.R));
firstHitPos = moveCap.Line.StartPos;
/// 三角形との最初に衝突する移動ラインを作成
calLine.Set(firstHitPos, firstHitMovePos);
}
/// 移動ラインと平面との交差チェック
if (isCrossFlg == true || checkRayCrossPlane(trgTri.Plane, calLine) == true)
{
/// 平面との交点を求める
trgDis = getRayPlaneCrossPoint(trgTri.Plane, calLine, ref collPos);
/// 三角形の内外判定
///-----------------------------------------------------
if (checkInsideTriangle(trgTri, collPos) == true)
{
return(true);
}
/// カプセルが三角形の３辺と接触しているかのチェック
///-----------------------------------------------------
else
{
/// 三角形の３辺と点との最近接点を取得
Vector3 linePos = new Vector3(0, 0, 0);
getClosestPtPosTriangle(collPos, trgTri, ref linePos);
Vector3 calVec = moveCap.Line.Vec * -1.0f;
/// 球が三角形の交点と接触するかチェック
calSph.Set(moveCap.Line.StartPos, moveCap.R);
float aT = checkRayCrossSphere(linePos, calVec, calSph);
trgDis = FMath.Sqrt(calSph.Pos.Dot(linePos));
if (trgDis < calSph.R)
{
aT = trgDis;
}
/// 衝突確定
if (aT >= 0.0f && aT <= moveCap.Line.Length)
{
collPos = linePos;
return(true);
}
}
}
return(false);
}
/// 点と三角形の３辺との最近接点を算出
private static float getClosestPtPosTriangle( Vector3 trgPos, GeometryTriangle trgTri, ref Vector3 cPos )
{
float isDis, bestDis;
Vector3 crosePos = new Vector3(0,0,0);
bestDis = 0.0f;
for( int i=0; i<3; i++ ){
isDis = getClosestPtPosLine( trgPos, trgTri.GetPos(i), trgTri.GetPos((i+1)%3), ref crosePos );
/// 最近接点を更新
if( i == 0 || isDis < bestDis ){
bestDis = isDis;
cPos = crosePos;
}
}
return bestDis;
}
/// 三角形と点との内外判定
private static bool checkInsideTriangle( GeometryTriangle trgTri, Vector3 crossPos )
{
int type;
float d1, d2, d3;
if( FMath.Abs( trgTri.Plane.Nor.X ) < FMath.Abs( trgTri.Plane.Nor.Y ) ){
type = (FMath.Abs( trgTri.Plane.Nor.Y ) < FMath.Abs( trgTri.Plane.Nor.Z ) ) ? 3 : 2;
}
else{
type = (FMath.Abs( trgTri.Plane.Nor.X ) < FMath.Abs( trgTri.Plane.Nor.Z )) ? 3 : 1;
}
switch( type ){
/// X方向に面が傾いているためX軸を破棄
case 1:
d1 = ( (crossPos.Y - trgTri.Pos1.Y) * (crossPos.Z - trgTri.Pos2.Z) ) -
( (crossPos.Z - trgTri.Pos1.Z) * (crossPos.Y - trgTri.Pos2.Y) );
d2 = ( (crossPos.Y - trgTri.Pos2.Y) * (crossPos.Z - trgTri.Pos3.Z) ) -
( (crossPos.Z - trgTri.Pos2.Z) * (crossPos.Y - trgTri.Pos3.Y) );
d3 = ( (crossPos.Y - trgTri.Pos3.Y) * (crossPos.Z - trgTri.Pos1.Z) ) -
( (crossPos.Z - trgTri.Pos3.Z) * (crossPos.Y - trgTri.Pos1.Y) );
break;
/// y方向に面が傾いているためY軸を破棄
case 2:
d1 = ( (crossPos.X - trgTri.Pos1.X) * (crossPos.Z - trgTri.Pos2.Z) ) -
( (crossPos.Z - trgTri.Pos1.Z) * (crossPos.X - trgTri.Pos2.X) );
d2 = ( (crossPos.X - trgTri.Pos2.X) * (crossPos.Z - trgTri.Pos3.Z) ) -
( (crossPos.Z - trgTri.Pos2.Z) * (crossPos.X - trgTri.Pos3.X) );
d3 = ( (crossPos.X - trgTri.Pos3.X) * (crossPos.Z - trgTri.Pos1.Z) ) -
( (crossPos.Z - trgTri.Pos3.Z) * (crossPos.X - trgTri.Pos1.X) );
break;
/// Z方向に面が傾いているためZ軸を破棄
case 3:
d1 = ( (crossPos.X - trgTri.Pos1.X) * (crossPos.Y - trgTri.Pos2.Y) ) -
( (crossPos.Y - trgTri.Pos1.Y) * (crossPos.X - trgTri.Pos2.X) );
d2 = ( (crossPos.X - trgTri.Pos2.X) * (crossPos.Y - trgTri.Pos3.Y) ) -
( (crossPos.Y - trgTri.Pos2.Y) * (crossPos.X - trgTri.Pos3.X) );
d3 = ( (crossPos.X - trgTri.Pos3.X) * (crossPos.Y - trgTri.Pos1.Y) ) -
( (crossPos.Y - trgTri.Pos3.Y) * (crossPos.X - trgTri.Pos1.X) );
break;
default:
return true;
}
/// 全て符号が同じなら当り。
if( (d1 >= -epsilon && d2 >= -epsilon && d3 >= -epsilon ) ||
(d1 <= epsilon && d2 <= epsilon && d3 <= epsilon ) ){
return true;
}
return false;
}
/// public メンバ （球の移動）
///---------------------------------------------------------------------------
/// 球の移動と三角形との衝突チェック
/**
* moveCapの軌道による球の移動とtrgTriとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
public static bool CheckSphereAndTriangle( GeometryCapsule moveCap, GeometryTriangle trgTri, ref Vector3 collPos )
{
/// 表裏判定
if( moveCap.Line.Vec.Dot( trgTri.Plane.Nor ) >= 0.0f ){
return false;
}
Vector3 firstHitPos, firstHitMovePos;
float trgDis;
bool isCrossFlg;
/// 三角形の平面との球の最近接点（頂点P）を算出
firstHitPos.X = moveCap.Line.StartPos.X + ( moveCap.R * (trgTri.Plane.Nor.X * -1) );
firstHitPos.Y = moveCap.Line.StartPos.Y + ( moveCap.R * (trgTri.Plane.Nor.Y * -1) );
firstHitPos.Z = moveCap.Line.StartPos.Z + ( moveCap.R * (trgTri.Plane.Nor.Z * -1) );
/// 既にPと球の中心が三角形を跨いでいるかチェック
isCrossFlg = false;
/// 符合が一致すると平面に対して交差した事にならない
float sign1, sign2;
sign1 = trgTri.Plane.Nor.Dot( moveCap.Line.StartPos ) + trgTri.Plane.D;
sign2 = trgTri.Plane.Nor.Dot( firstHitPos ) + trgTri.Plane.D;
if( (sign1 > epsilon && sign2 > epsilon) || (sign1 < epsilon && sign2 < epsilon) ){
calLine.Vec = moveCap.Line.Vec;
calLine.Length = moveCap.Line.Length;
calLine.StartPos = firstHitPos;
/// Pが移動する地点P'を算出
calLine.EndPos.X = firstHitPos.X + ( moveCap.Line.Vec.X * moveCap.Line.Length );
calLine.EndPos.Y = firstHitPos.Y + ( moveCap.Line.Vec.Y * moveCap.Line.Length );
calLine.EndPos.Z = firstHitPos.Z + ( moveCap.Line.Vec.Z * moveCap.Line.Length );
}
else{
isCrossFlg = true;
/// Pが移動する地点P'を算出
firstHitMovePos.X = moveCap.Line.StartPos.X + ( moveCap.Line.Vec.X * (moveCap.Line.Length+moveCap.R) );
firstHitMovePos.Y = moveCap.Line.StartPos.Y + ( moveCap.Line.Vec.Y * (moveCap.Line.Length+moveCap.R) );
firstHitMovePos.Z = moveCap.Line.StartPos.Z + ( moveCap.Line.Vec.Z * (moveCap.Line.Length+moveCap.R) );
firstHitPos = moveCap.Line.StartPos;
/// 三角形との最初に衝突する移動ラインを作成
calLine.Set( firstHitPos, firstHitMovePos );
}
/// 移動ラインと平面との交差チェック
if( isCrossFlg == true || checkRayCrossPlane( trgTri.Plane, calLine ) == true ){
/// 平面との交点を求める
trgDis = getRayPlaneCrossPoint( trgTri.Plane, calLine, ref collPos );
/// 三角形の内外判定
///-----------------------------------------------------
if( checkInsideTriangle( trgTri, collPos ) == true ){
return true;
}
/// カプセルが三角形の３辺と接触しているかのチェック
///-----------------------------------------------------
else{
/// 三角形の３辺と点との最近接点を取得
Vector3 linePos = new Vector3(0,0,0);
getClosestPtPosTriangle( collPos, trgTri, ref linePos );
Vector3 calVec = moveCap.Line.Vec * -1.0f;
/// 球が三角形の交点と接触するかチェック
calSph.Set( moveCap.Line.StartPos, moveCap.R );
float aT = checkRayCrossSphere( linePos, calVec, calSph );
trgDis = FMath.Sqrt( calSph.Pos.Dot( linePos ) );
if( trgDis < calSph.R ){
aT = trgDis;
}
/// 衝突確定
if( aT >= 0.0f && aT <= moveCap.Line.Length ){
collPos = linePos;
return true;
}
}
}
return false;
}
/// 点の移動と三角形との衝突チェック
/**
* 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 void DrawTriangle( GraphicsContext graphics, GeometryTriangle trgTri, Camera cam, Rgba color )
{
if( debShader == null ){
return ;
}
/// バーテクスの更新
///------------------------------------------------------------
for( int i=0; i<3; i++ ){
debMesh.Positions[ i*3+0 ] = trgTri.GetPos(i).X;
debMesh.Positions[ i*3+1 ] = trgTri.GetPos(i).Y;
debMesh.Positions[ i*3+2 ] = trgTri.GetPos(i).Z;
debMesh.Normals[ i*3+0 ] = trgTri.Plane.Nor.X;
debMesh.Normals[ i*3+1 ] = trgTri.Plane.Nor.Y;
debMesh.Normals[ i*3+2 ] = trgTri.Plane.Nor.Z;
}
debVb.SetVertices( 0, debMesh.Positions );
debVb.SetIndices( debMesh.Indices );
drawMesh( graphics, cam, color );
}
