void Transform2x2Rows
( const Matrix<T>& G, AbstractDistMatrix<T>& A, Int i1, Int i2 )
{
DEBUG_CSE
const int rowOwner1 = A.RowOwner(i1);
const int rowOwner2 = A.RowOwner(i2);
const bool inFirstRow = ( A.ColRank() == rowOwner1 );
const bool inSecondRow = ( A.ColRank() == rowOwner2 );
if( !inFirstRow && !inSecondRow )
return;
T* ABuf = A.Buffer();
const Int ALDim = A.LDim();
const Int nLoc = A.LocalWidth();
const T gamma11 = G(0,0);
const T gamma12 = G(0,1);
const T gamma21 = G(1,0);
const T gamma22 = G(1,1);
if( inFirstRow && inSecondRow )
{
const Int i1Loc = A.LocalRow(i1);
const Int i2Loc = A.LocalRow(i2);
Transform2x2
( nLoc,
gamma11, gamma12, gamma21, gamma22,
&ABuf[i1Loc], ALDim, &ABuf[i2Loc], ALDim );
}
else if( inFirstRow )
{
const Int i1Loc = A.LocalRow(i1);
vector<T> buf(nLoc);
for( Int jLoc=0; jLoc<nLoc; ++jLoc )
buf[jLoc] = ABuf[i1Loc+jLoc*ALDim];
mpi::SendRecv( buf.data(), nLoc, rowOwner2, rowOwner2, A.ColComm() );
// TODO: Generalized Axpy?
blas::Scal( nLoc, gamma11, &ABuf[i1Loc], ALDim );
blas::Axpy( nLoc, gamma12, buf.data(), 1, &ABuf[i1Loc], ALDim );
}
else
{
const Int i2Loc = A.LocalRow(i2);
vector<T> buf(nLoc);
for( Int jLoc=0; jLoc<nLoc; ++jLoc )
buf[jLoc] = ABuf[i2Loc+jLoc*ALDim];
mpi::SendRecv( buf.data(), nLoc, rowOwner1, rowOwner1, A.ColComm() );
// TODO: Generalized Axpy?
blas::Scal( nLoc, gamma22, &ABuf[i2Loc], ALDim );
blas::Axpy( nLoc, gamma21, buf.data(), 1, &ABuf[i2Loc], ALDim );
}
}
void Transform2x2Rows
( const Matrix<T>& G,
Matrix<T>& A, Int i1, Int i2 )
{
DEBUG_CSE
auto a1 = A( IR(i1), ALL );
auto a2 = A( IR(i2), ALL );
Transform2x2( G, a1, a2 );
}
void Transform2x2
( const AbstractDistMatrix<T>& GPre,
AbstractDistMatrix<T>& a1,
AbstractDistMatrix<T>& a2 )
{
DEBUG_CSE
DistMatrixReadProxy<T,T,STAR,STAR> GProx( GPre );
const auto& G = GProx.GetLocked();
Transform2x2( G.LockedMatrix(), a1, a2 );
}
void Transform2x2Cols
( const Matrix<T>& G, Matrix<T>& A, Int i1, Int i2 )
{
DEBUG_CSE
// Since the scalar version of Transform2x2 assumes that a1 and a2 are
// row vectors, we implicitly transpose G on input to it so that we can
// apply [a1, a2] G via G^T [a1^T; a2^T].
Transform2x2
( A.Height(), G(0,0), G(1,0), G(0,1), G(1,1),
A.Buffer(0,i1), 1, A.Buffer(0,i2), 1 );
}
void Transform2x2( const Matrix<T>& G, Matrix<T>& a1, Matrix<T>& a2 )
{
DEBUG_CSE
T* a1Buf = a1.Buffer();
T* a2Buf = a2.Buffer();
const Int inc1 = ( a1.Height() == 1 ? a1.LDim() : 1 );
const Int inc2 = ( a2.Height() == 1 ? a2.LDim() : 1 );
const Int n = ( a1.Height() == 1 ? a1.Width() : a1.Height() );
const T gamma11 = G.Get(0,0);
const T gamma12 = G.Get(0,1);
const T gamma21 = G.Get(1,0);
const T gamma22 = G.Get(1,1);
Transform2x2
( n, gamma11, gamma12, gamma21, gamma22, a1Buf, inc1, a2Buf, inc2 );
}
// Test intersection with a swept ellipsoid with principal axis inAxis1, inAxis2, inAxis3 moving from inBegin to inBegin + inDelta
// If there is an intersection the intersection position is returned in outPoint and the center of the
// sphere is at inBegin + outFraction * inDelta when it collides
bool PolygonSweptEllipsoidIntersect(const Plane &inPlane, const Vector2 *inVertices, int inNumVertices, const Vector3 &inBegin, const Vector3 &inDelta, const Vector3 &inAxis1, const Vector3 &inAxis2, const Vector3 &inAxis3, Vector3 &outPoint, float &outFraction)
{
// Compute matrix that takes a point from unit sphere space to world space
// NOTE: When colliding with lots of polygons this can be cached
Matrix unit_sphere_to_world;
unit_sphere_to_world.Column(0) = inAxis1;
unit_sphere_to_world.Column(1) = inAxis2;
unit_sphere_to_world.Column(2) = inAxis3;
// Compute matrix that takes a point from world space to unit sphere space
// NOTE: When colliding with lots of polygons this can be cached
Matrix world_to_unit_sphere = unit_sphere_to_world.GetInversed();
// Compute begin and delta in unit sphere space
// NOTE: When colliding with lots of polygons this can be cached
Vector3 begin_uss = world_to_unit_sphere * inBegin;
Vector3 delta_uss = world_to_unit_sphere * inDelta;
// Transform the plane into unit sphere local space
Plane transformed_plane;
transformed_plane = inPlane.GetTransformedByInverse(unit_sphere_to_world);
// Determine the range over which the unit sphere intersects the transformed plane
float t1, t2;
if (!PlaneSweptSphereIntersect(transformed_plane, begin_uss, delta_uss, 1.0f, t1, t2))
return false;
// Get matrix that transforms a point from plane space to world space
Matrix plane_to_world = inPlane.GetPlaneToWorldMatrix();
// Get matrix that transforms a point from the transformed plane to unit sphere space
Matrix transformed_plane_to_unit_sphere = transformed_plane.GetPlaneToWorldMatrix();
// Get matrix that takes a 2d polygon vertex from the original space to the space of the
// transformed plane so that the unit sphere is still a unit sphere
Matrix plane_to_transformed_plane = transformed_plane_to_unit_sphere.GetInversed() * world_to_unit_sphere * plane_to_world;
// The radius of the circle is defined as: radius^2 = 1 - (distance plane to center)^2
// this can be written as: radius^2 = a * t^2 + b * t + c
float n_dot_d = transformed_plane.mNormal.Dot(delta_uss);
float dist_to_b = transformed_plane.GetSignedDistance(begin_uss);
float a = -n_dot_d * n_dot_d;
float b = -2.0f * n_dot_d * dist_to_b;
float c = 1.0f - dist_to_b * dist_to_b;
// Get the basis vectors for the transformed plane
const Vector3 &u = transformed_plane_to_unit_sphere.Column(0);
const Vector3 &v = transformed_plane_to_unit_sphere.Column(1);
// To avoid translating the polygon we subtract the translation from the begin point
// and then later add it to the collision result again
Vector2 trans(plane_to_transformed_plane.E(0, 3), plane_to_transformed_plane.E(1, 3));
// Get the equation for the intersection circle between the plane and the unit sphere: center = begin + t * delta
Vector2 begin = Plane::sConvertWorldToPlane(u, v, begin_uss) - trans;
Vector2 delta = Plane::sConvertWorldToPlane(u, v, delta_uss);
// Transform the polygon
Vector2 *transformed_vertices = (Vector2 *)alloca(inNumVertices * sizeof(Vector2));
for (int i = 0; i < inNumVertices; ++i)
transformed_vertices[i] = Transform2x2(plane_to_transformed_plane, inVertices[i]);
// Test if sphere intersects at t1
Vector2 p;
if (PolygonCircleIntersect(transformed_vertices, inNumVertices, begin + delta * t1, a * t1 * t1 + b * t1 + c, p))
{
outFraction = t1;
outPoint = unit_sphere_to_world * (transformed_plane_to_unit_sphere * Vector3(p + trans));
return true;
}
// Test if sphere intersects with one of the edges or vertices
if (SweptCircleEdgeVertexIntersect(transformed_vertices, inNumVertices, begin, delta, a, b, c, p, outFraction))
{
outPoint = unit_sphere_to_world * (transformed_plane_to_unit_sphere * Vector3(p + trans));
return true;
}
return false;
}
void Transform2x2Cols
( const Matrix<T>& G, AbstractDistMatrix<T>& A, Int j1, Int j2 )
{
DEBUG_CSE
const int colOwner1 = A.ColOwner(j1);
const int colOwner2 = A.ColOwner(j2);
const bool inFirstCol = ( A.RowRank() == colOwner1 );
const bool inSecondCol = ( A.RowRank() == colOwner2 );
if( !inFirstCol && !inSecondCol )
return;
T* ABuf = A.Buffer();
const Int ALDim = A.LDim();
const Int mLoc = A.LocalHeight();
vector<T> buf(mLoc);
const T gamma11 = G(0,0);
const T gamma12 = G(0,1);
const T gamma21 = G(1,0);
const T gamma22 = G(1,1);
if( inFirstCol && inSecondCol )
{
const Int j1Loc = A.LocalCol(j1);
const Int j2Loc = A.LocalCol(j2);
// Since the scalar version of Transform2x2 assumes that a1 and a2 are
// row vectors, we implicitly transpose G on input to it so that we can
// apply [a1, a2] G via G^T [a1^T; a2^T].
Transform2x2
( mLoc,
gamma11, gamma21, gamma12, gamma22,
&ABuf[j1Loc*ALDim], 1,
&ABuf[j2Loc*ALDim], 1 );
}
else if( inFirstCol )
{
const Int j1Loc = A.LocalCol(j1);
for( Int iLoc=0; iLoc<mLoc; ++iLoc )
buf[iLoc] = ABuf[iLoc+j1Loc*ALDim];
mpi::SendRecv( buf.data(), mLoc, colOwner2, colOwner2, A.RowComm() );
// TODO: Generalized Axpy?
blas::Scal( mLoc, gamma11, &ABuf[j1Loc*ALDim], 1 );
blas::Axpy( mLoc, gamma21, buf.data(), 1, &ABuf[j1Loc*ALDim], 1 );
}
else
{
const Int j2Loc = A.LocalCol(j2);
for( Int iLoc=0; iLoc<mLoc; ++iLoc )
buf[iLoc] = ABuf[iLoc+j2Loc*ALDim];
mpi::SendRecv( buf.data(), mLoc, colOwner1, colOwner1, A.RowComm() );
// TODO: Generalized Axpy?
blas::Scal( mLoc, gamma22, &ABuf[j2Loc*ALDim], 1 );
blas::Axpy( mLoc, gamma12, buf.data(), 1, &ABuf[j2Loc*ALDim], 1 );
}
}
