/// カプセル描画 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) { /// 線分同士の交点を用いて、2つの線分の角度を求める 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 ){ /// 線分同士の交点を用いて、2つの線分の角度を求める 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); } /// カプセルが三角形の3辺と接触しているかのチェック ///----------------------------------------------------- else { /// 三角形の3辺と点との最近接点を取得 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; } /// カプセルが三角形の3辺と接触しているかのチェック ///----------------------------------------------------- else{ /// 三角形の3辺と点との最近接点を取得 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; } }