void PlanarPortal::teleportPointThroughPlaneB( const Vector3s& xin, Vector3s& xout ) const
{
// Compute coordinates of x in B and invert them
const scalar nB {
m_portal_multiplier.x() * planeB().n().dot( planeB().x() - xin )
};
const scalar tB0{ m_portal_multiplier.y() * planeB().t0().dot( planeB().x() - xin ) };
const scalar tB1{ m_portal_multiplier.z() * planeB().t1().dot( planeB().x() - xin ) };
// Compute the inverted coordinates in B
xout = planeA().x() + nB * planeA().n() + tB0 * planeA().t0() + tB1 * planeA().t1();
}
Vector2s PlanarPortal::getKinematicVelocityOfPoint( const Vector2s& x ) const
{
assert( planeA().distanceLessThanZero( x ) != planeB().distanceLessThanZero( x ) );
if( planeA().distanceLessThanZero( x ) )
{
return -m_v * m_plane_a.t();
}
else
{
return -m_v * m_plane_b.t();
}
}
void PlanarPortal::teleportPointThroughPlaneB( const Vector2s& xin, Vector2s& xout ) const
{
// Compute the translated coordinates of x in B...
const scalar nB{ planeB().n().dot( planeB().x() - xin ) };
scalar tB{ planeB().t().dot( m_dx * m_plane_b.t() + planeB().x() - xin ) };
// ...accounting for periodic boundaries
const int repeat_x{ int( floor( ( tB + m_bounds ) / ( 2.0 * m_bounds ) ) ) };
tB -= 2.0 * repeat_x * m_bounds;
assert( tB >= -m_bounds || !isLeesEdwards() );
assert( tB <= m_bounds || !isLeesEdwards() );
// Map the coordinates to the corresponding plane
xout = planeA().x() + nB * planeA().n() + tB * planeA().t();
}
void PlanarPortal::teleportPointInsidePortal( const Vector2s& xin, Vector2s& xout ) const
{
assert( planeA().distanceLessThanZero( xin ) != planeB().distanceLessThanZero( xin ) );
// Teleporting form A to B
if( planeA().distanceLessThanZero( xin ) )
{
teleportPointThroughPlaneA( xin, xout );
}
// Teleporting form B to A
else
{
teleportPointThroughPlaneB( xin, xout );
}
}
void PlanarPortal::teleportPointInsidePortal( const Vector3s& xin, Vector3s& xout ) const
{
// Point must be in one and only one side of the portal
assert( ( planeA().distanceToPoint( xin ) < 0 ) != ( planeB().distanceToPoint( xin ) < 0 ) );
// Teleporting form A to B
if( planeA().distanceToPoint( xin ) < 0 )
{
teleportPointThroughPlaneA( xin, xout );
}
// Teleporting form B to A
else
{
teleportPointThroughPlaneB( xin, xout );
}
}
void rayPlaneCollisionTests() {
vxVector planeA(-5, 0, 0);
vxVector planeB(5, 0, 0);
vxVector planeC(5, 5, 0);
// hit going forward
vxRay testRay(vxVector(0, 2.5, -5), vxVector(0, 0, 1));
assert(testRay.intersect(planeA, planeB, planeC).hit);
// see if it hits even if the direction is backward
vxRay infiniteHitRay(vxVector(0, 2.5, -5), vxVector(0, 0, -1));
assert(infiniteHitRay.intersect(planeA, planeB, planeC).hit);
}
void PlanarPortal::teleportPointThroughPlaneB( const Vector2s& xin, Vector2s& xout ) const
{
// Compute coordinates of x in B and invert them
const scalar nB{ planeB().n().dot( m_dx_b * m_plane_b.t() + planeB().x() - xin ) };
scalar tB{ planeB().t().dot( m_dx_b * m_plane_b.t() + planeB().x() - xin ) };
// Compute the inverted coordinates in B
if( m_dx_a + tB < m_bounds_a(0) )
{
while( m_dx_a + tB < m_bounds_a(0) )
{
tB += m_bounds_a(1) - m_bounds_a(0);
}
}
else if( m_dx_a + tB > m_bounds_a(1) )
{
while( m_dx_a + tB > m_bounds_a(1) )
{
tB += m_bounds_a(0) - m_bounds_a(1);
}
}
assert( m_dx_a + tB >= m_bounds_a(0) ); assert( m_dx_a + tB <= m_bounds_a(1) );
xout = m_dx_a * m_plane_a.t() + planeA().x() + nB * planeA().n() + tB * planeA().t();
}
bool PlanarPortal::sphereTouchesPortal( const Vector3s& x, const scalar& r, bool& intersecting_plane_idx ) const
{
const bool touches_plane_A{ planeA().distanceToPoint( x ) <= r };
const bool touches_plane_B{ planeB().distanceToPoint( x ) <= r };
if( touches_plane_A && touches_plane_B )
{
std::cerr << "Unhandled case in PlanarPortal::sphereTouchesPortal. Sphere can't touch both planes of a portal at once." << std::endl;
std::exit( EXIT_FAILURE );
}
if( touches_plane_A )
{
intersecting_plane_idx = 0;
}
if( touches_plane_B )
{
intersecting_plane_idx = 1;
}
return touches_plane_A || touches_plane_B;
}
static SCE_PFX_FORCE_INLINE
bool pfxContactTriangleSphere(PfxContactCache &contacts,PfxUInt32 facetId,
const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
PfxUInt32 edgeChk,
PfxFloat sphereRadius,const PfxVector3 &spherePos)
{
PfxVector3 facetPnts[3] = {
p0,p1,p2,
};
// 早期判定
{
PfxPlane planeA(normal,p0);
PfxFloat len1 = planeA.onPlane(spherePos);
if(len1 >= sphereRadius || len1 < -thickness-sphereRadius) return false;
}
// 球と面の最近接点を計算
{
PfxTriangle triangleA(p0,p1,p2);
PfxVector3 pntA;
// pfxClosestPointTriangle(spherePos,triangleA,pntA);
bool insideTriangle = false;
while(1) {
PfxVector3 ab = p1 - p0;
PfxVector3 ac = p2 - p0;
PfxVector3 ap = spherePos - p0;
PfxFloat d1 = dot(ab, ap);
PfxFloat d2 = dot(ac, ap);
if(d1 <= 0.0f && d2 <= 0.0f) {
pntA = p0;
break;
}
PfxVector3 bp = spherePos - p1;
PfxFloat d3 = dot(ab, bp);
PfxFloat d4 = dot(ac, bp);
if (d3 >= 0.0f && d4 <= d3) {
pntA = p1;
break;
}
PfxFloat vc = d1*d4 - d3*d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
PfxFloat v = d1 / (d1 - d3);
pntA = p0 + v * ab;
break;
}
PfxVector3 cp = spherePos - p2;
PfxFloat d5 = dot(ab, cp);
PfxFloat d6 = dot(ac, cp);
if (d6 >= 0.0f && d5 <= d6) {
pntA = p2;
break;
}
PfxFloat vb = d5*d2 - d1*d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
PfxFloat w = d2 / (d2 - d6);
pntA = p0 + w * ac;
break;
}
PfxFloat va = d3*d6 - d5*d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
pntA = p1 + w * (p2 - p1);
break;
}
PfxFloat den = 1.0f / (va + vb + vc);
PfxFloat v = vb * den;
PfxFloat w = vc * den;
pntA = p0 + ab * v + ac * w;
insideTriangle = true;
break;
}
PfxVector3 distVec = pntA - spherePos;
PfxFloat l = length(distVec);
if(!insideTriangle && l >= sphereRadius) return false;
// 分離軸
PfxVector3 sepAxis = (l < 0.00001f || insideTriangle) ? -normal : distVec / l;
// 球上の衝突点
PfxVector3 pointsOnSphere = spherePos + sphereRadius * sepAxis;
PfxVector3 pointsOnTriangle = pntA;
// 面上の最近接点が凸エッジ上でない場合は法線を変える
if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
sepAxis=-normal;
}
PfxSubData subData;
subData.setFacetId(facetId);
contacts.addContactPoint(-length(pointsOnSphere-pointsOnTriangle),sepAxis,PfxPoint3(pointsOnTriangle),PfxPoint3(pointsOnSphere),subData);
}
return true;
}
bool PlanarPortal::pointInsidePortal( const Vector2s& x ) const
{
return planeA().distanceLessThanZero( x ) || planeB().distanceLessThanZero( x );
}
bool PlanarPortal::pointInsidePortal( const Vector2s& x ) const
{
return planeA().distanceLessThanZero( m_dx_a * m_plane_a.t(), x ) || planeB().distanceLessThanZero( m_dx_b * m_plane_b.t(), x );
}
bool PlanarPortal::pointInsidePortal( const Vector3s& x ) const
{
return ( planeA().distanceToPoint( x ) < 0 ) || ( planeB().distanceToPoint( x ) < 0 );
}
