/// カプセル描画
public void DrawCapsule(GraphicsContext graphics, GeometryCapsule trgCap, Camera cam, Rgba color)
{
if (debShader == null || trgCap.R <= 0.00001f)
{
return;
}
/// 球体部分
///---------------------------------------------
DrawSphere(graphics, new GeometrySphere(trgCap.StartPos, trgCap.R), cam, color);
DrawSphere(graphics, new GeometrySphere(trgCap.EndPos, trgCap.R), cam, color);
/// パイプ部分
///---------------------------------------------
setCapsulePipeVertex(trgCap);
debVb2.SetVertices(0, debMesh2.Positions);
debVb2.SetIndices(debMesh2.Indices);
Matrix4 localMtx;
if (trgCap.StartPos.X == trgCap.EndPos.X && trgCap.StartPos.Z == trgCap.EndPos.Z)
{
localMtx = Matrix4.LookAt(trgCap.EndPos, trgCap.StartPos, new Vector3(0.0f, 0.0f, 1.0f));
}
else
{
localMtx = Matrix4.LookAt(trgCap.EndPos, trgCap.StartPos, new Vector3(0.0f, 1.0f, 0.0f));
}
Matrix4 world = localMtx.Inverse() * Matrix4.Scale(new Vector3(trgCap.R, trgCap.R, trgCap.R));
world.M41 = trgCap.StartPos.X;
world.M42 = trgCap.StartPos.Y;
world.M43 = trgCap.StartPos.Z;
Matrix4 worldViewProj = cam.Projection * cam.View * world;
// uniform value
debShader.SetUniformValue(debUIdWVP, ref worldViewProj);
Vector4 a_Color = new Vector4((color.R / 255.0f), (color.G / 255.0f), (color.B / 255.0f), (color.A / 255.0f));
debShader.SetUniformValue(debShader.FindUniform("IAmbient"), ref a_Color);
graphics.SetShaderProgram(debShader);
graphics.SetVertexBuffer(0, debVb2);
graphics.DrawArrays(debMesh2.Prim, 0, debMesh2.IndexCount);
}
/// カプセルのパイプ部分の頂点設定
public void setCapsulePipeVertex(GeometryCapsule trgCap)
{
Vector3 MoveVec = trgCap.EndPos - trgCap.StartPos;
float height = FMath.Sqrt(MoveVec.Dot(MoveVec));
int a_pos_cnt = 0;
height *= (1.0f / trgCap.R);
for (int j = 0; j < debSphDiv * 2; j++)
{
debMesh2.Positions[a_pos_cnt + 2] = height;
a_pos_cnt += 6;
}
}
/// 球の移動とカプセルとの衝突チェック
/**
* moveCapの軌道での球の移動とtrgCapとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
static public bool CheckCapsuleAndCapsule(GeometryCapsule moveCap, GeometryCapsule trgCap, ref Vector3 collPos)
{
// Vector3 c1, c2;
Vector3 c1 = new Vector3(0, 0, 0);
Vector3 c2 = new Vector3(0, 0, 0);
float dist = getClosestPtLineLine(moveCap.Line, trgCap.Line, ref c1, ref c2);
float radius = moveCap.R + trgCap.R;
/// カプセル同士の衝突は無し
if (dist > radius * radius)
{
return(false);
}
/// カプセル同士の衝突点の算出
///-----------------------------------------------
/// カプセルの中心の線分同士が交わる
if (dist <= epsilon)
{
/// 線分同士の交点を用いて、２つの線分の角度を求める
float rad = getRadian(c1, moveCap.Line.StartPos, trgCap.Line.EndPos);
/// 衝突点を求める
float sin = FMath.Sin(rad);
if (sin > 0.0f)
{
float dis = radius / sin;
collPos = (moveCap.Vec * dis * -1) + c1;
}
else
{
collPos = (moveCap.Vec * radius * -1) + c1;
}
}
/// カプセルの外側の球に接触する
else
{
/// 対象カプセルの中心線分の最近接点に球体を生成、その球体と移動する球の軌道との衝突を行い衝突点を算出する
calSph.Set(c2, (moveCap.R + trgCap.R));
CheckLineAndSphere(moveCap.Line, calSph, ref collPos);
}
return(true);
}
/// 球の移動とカプセルとの衝突チェック
/**
* moveCapの軌道での球の移動とtrgCapとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
public static bool CheckCapsuleAndCapsule( GeometryCapsule moveCap, GeometryCapsule trgCap, ref Vector3 collPos )
{
// Vector3 c1, c2;
Vector3 c1 = new Vector3(0,0,0);
Vector3 c2 = new Vector3(0,0,0);
float dist = getClosestPtLineLine( moveCap.Line, trgCap.Line, ref c1, ref c2 );
float radius = moveCap.R + trgCap.R;
/// カプセル同士の衝突は無し
if( dist > radius*radius ){
return false;
}
/// カプセル同士の衝突点の算出
///-----------------------------------------------
/// カプセルの中心の線分同士が交わる
if( dist <= epsilon ){
/// 線分同士の交点を用いて、２つの線分の角度を求める
float rad = getRadian( c1, moveCap.Line.StartPos, trgCap.Line.EndPos );
/// 衝突点を求める
float sin = FMath.Sin( rad );
if( sin > 0.0f ){
float dis = radius / sin;
collPos = (moveCap.Vec * dis * -1) + c1;
}
else{
collPos = (moveCap.Vec * radius * -1) + c1;
}
}
/// カプセルの外側の球に接触する
else{
/// 対象カプセルの中心線分の最近接点に球体を生成、その球体と移動する球の軌道との衝突を行い衝突点を算出する
calSph.Set( c2, (moveCap.R+trgCap.R) );
CheckLineAndSphere( moveCap.Line, calSph, ref collPos );
}
return true;
}
/// 球の移動と球との衝突チェック
/**
* moveCapの軌道による球の移動とtrgSphとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
static public bool CheckSphereAndSphere(GeometryCapsule moveCap, GeometrySphere trgSph, ref Vector3 collPos)
{
calSph.Set(trgSph.Pos, (moveCap.R + trgSph.R));
return(CheckLineAndSphere(moveCap.Line, calSph, ref collPos));
}
/// 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);
}
/// 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;
}
/// 球の移動と球との衝突チェック
/**
* moveCapの軌道による球の移動とtrgSphとの衝突チェックを行います。
* collPosには衝突した座標を返します（返り値がtrueの時のみ情報が更新）
*/
public static bool CheckSphereAndSphere( GeometryCapsule moveCap, GeometrySphere trgSph, ref Vector3 collPos )
{
calSph.Set( trgSph.Pos, (moveCap.R + trgSph.R) );
return( CheckLineAndSphere( moveCap.Line, calSph, ref collPos ) );
}
/// カプセル描画
public void DrawCapsule( GraphicsContext graphics, GeometryCapsule trgCap, Camera cam, Rgba color )
{
if( debShader == null || trgCap.R <= 0.00001f ){
return ;
}
/// 球体部分
///---------------------------------------------
DrawSphere( graphics, new GeometrySphere( trgCap.StartPos, trgCap.R ), cam, color );
DrawSphere( graphics, new GeometrySphere( trgCap.EndPos, trgCap.R ), cam, color );
/// パイプ部分
///---------------------------------------------
setCapsulePipeVertex( trgCap );
debVb2.SetVertices( 0, debMesh2.Positions );
debVb2.SetIndices( debMesh2.Indices );
Matrix4 localMtx;
if( trgCap.StartPos.X == trgCap.EndPos.X && trgCap.StartPos.Z == trgCap.EndPos.Z ){
localMtx = Matrix4.LookAt( trgCap.EndPos, trgCap.StartPos, new Vector3(0.0f, 0.0f, 1.0f));
}
else{
localMtx = Matrix4.LookAt( trgCap.EndPos, trgCap.StartPos, new Vector3(0.0f, 1.0f, 0.0f));
}
Matrix4 world = localMtx.Inverse() * Matrix4.Scale( new Vector3( trgCap.R, trgCap.R, trgCap.R ) );
world.M41 = trgCap.StartPos.X;
world.M42 = trgCap.StartPos.Y;
world.M43 = trgCap.StartPos.Z;
Matrix4 worldViewProj = cam.Projection * cam.View * world;
// uniform value
debShader.SetUniformValue( debUIdWVP, ref worldViewProj );
Vector4 a_Color = new Vector4( (color.R / 255.0f), (color.G / 255.0f), (color.B / 255.0f), (color.A / 255.0f) );
debShader.SetUniformValue( debShader.FindUniform( "IAmbient" ), ref a_Color );
graphics.SetShaderProgram( debShader );
graphics.SetVertexBuffer( 0, debVb2 );
graphics.DrawArrays( debMesh2.Prim, 0, debMesh2.IndexCount );
}
/// カプセルのパイプ部分の頂点設定
public void setCapsulePipeVertex( GeometryCapsule trgCap )
{
Vector3 MoveVec = trgCap.EndPos-trgCap.StartPos;
float height = FMath.Sqrt( MoveVec.Dot(MoveVec) );
int a_pos_cnt = 0;
height *= (1.0f / trgCap.R);
for( int j = 0; j < debSphDiv*2; j++ ){
debMesh2.Positions[ a_pos_cnt + 2 ] = height;
a_pos_cnt += 6;
}
}
