void CDynamics2DEngine::PositionPhysicsToSpace(CVector3& c_new_pos,
const CVector3& c_original_pos,
const cpBody* pt_body) {
c_new_pos.SetX(pt_body->p.x);
c_new_pos.SetY(pt_body->p.y);
c_new_pos.SetZ(c_original_pos.GetZ());
}
bool CBuzzControllerSpiri::Land() {
CVector3 cPos = m_pcPosition->GetReading().Position;
if(Abs(cPos.GetZ()) < 0.01f) return false;
cPos.SetZ(0.0f);
m_pcPropellers->SetAbsolutePosition(cPos);
return true;
}
// --[ Method ]---------------------------------------------------------------
//
// - Class : CMatrix
//
// - prototype : CMatrix Quaternion()
//
// - Purpose : Returns the equivalent quaternion.
//
// -----------------------------------------------------------------------------
CQuaternion CMatrix::Quaternion() const
{
CQuaternion result;
float fTrace, fS;
fTrace = 1.0f + m_fM[0][0] + m_fM[1][1] + m_fM[2][2];
// Check trace value
if(fTrace > 0.000001f)
{
fS = 0.5f / sqrtf(fTrace);
result.SetW(0.25f / fS);
CVector3 v;
v.SetX((m_fM[2][1] - m_fM[1][2]) * fS);
v.SetY((m_fM[0][2] - m_fM[2][0]) * fS);
v.SetZ((m_fM[1][0] - m_fM[0][1]) * fS);
result.SetV(v);
return result;
}
else
{
float qx, qy, qz, qw;
if(m_fM[0][0] > m_fM[1][1] && m_fM[0][0] > m_fM[2][2])
{
fS = sqrtf(1.0f + m_fM[0][0] - m_fM[1][1] - m_fM[2][2]) * 2.0f;
qx = 0.25f * fS;
qy = (m_fM[0][1] + m_fM[1][0] ) / fS;
qz = (m_fM[0][2] + m_fM[2][0] ) / fS;
qw = (m_fM[1][2] - m_fM[2][1] ) / fS;
}
else if(m_fM[1][1] > m_fM[2][2])
{
fS = sqrt( 1.0 + m_fM[1][1] - m_fM[0][0] - m_fM[2][2] ) * 2;
qx = (m_fM[0][1] + m_fM[1][0] ) / fS;
qy = 0.25f * fS;
qz = (m_fM[1][2] + m_fM[2][1] ) / fS;
qw = (m_fM[0][2] - m_fM[2][0] ) / fS;
}
else
{
fS = sqrt( 1.0 + m_fM[2][2] - m_fM[0][0] - m_fM[1][1] ) * 2;
qx = (m_fM[0][2] + m_fM[2][0] ) / fS;
qy = (m_fM[1][2] + m_fM[2][1] ) / fS;
qz = 0.25f * fS;
qw = (m_fM[0][1] - m_fM[1][0] ) / fS;
}
result.SetW(qw);
result.SetV(CVector3(qx, qy, qz));
}
return result;
}
CVector3 AABox::Corner( int idx ) const
{
CVector3 rval;
rval.SetX( ((idx & 1) == 1) ? mMax.GetX() : mMin.GetX() );
rval.SetY( ((idx & 2) == 1) ? mMax.GetY() : mMin.GetY() );
rval.SetZ( ((idx & 4) == 1) ? mMax.GetZ() : mMin.GetZ() );
return rval;
}
CRay3 VoronoiDiagram::ToVoronoiEdge(const Edge& edge) const {
CRay3 voronoiEdge;
if (edge.is_finite()) {
voronoiEdge.SetStart(ToVector3(*edge.vertex0()));
voronoiEdge.SetEnd(ToVector3(*edge.vertex1()));
}
else {
const auto& cell1 = *edge.cell();
const auto& cell2 = *edge.twin()->cell();
VoronoiDiagram::Point origin, direction;
VoronoiDiagram::Point p1 = boostPoints.at(cell1.source_index());
VoronoiDiagram::Point p2 = boostPoints.at(cell2.source_index());
p1.set(HORIZONTAL, p1.x()/scaleVectorToMilimeters);
p1.set(VERTICAL, p1.y()/scaleVectorToMilimeters);
p2.set(HORIZONTAL, p2.x()/scaleVectorToMilimeters);
p2.set(VERTICAL, p2.y()/scaleVectorToMilimeters);
origin.x((p1.x() + p2.x()) * 0.5);
origin.y((p1.y() + p2.y()) * 0.5);
direction.x(p1.y() - p2.y());
direction.y(p2.x() - p1.x());
Real side = arenaLimits.GetMax().GetX()*2;
Real koef = side / max(fabs(direction.x()), fabs(direction.y()));
if (edge.vertex0() == NULL) {
CVector3 start;
start.SetX(origin.x() - (direction.x() * koef));
start.SetY(origin.y() - (direction.y() * koef));
start.SetZ(diagramLiftOnZ);
voronoiEdge.SetStart(start);
} else {
voronoiEdge.SetStart(ToVector3(*edge.vertex0()));
}
if (edge.vertex1() == NULL) {
CVector3 end;
end.SetX(origin.x() + direction.x() * koef);
end.SetY(origin.y() + direction.y() * koef);
end.SetZ(diagramLiftOnZ);
voronoiEdge.SetEnd(end);
} else {
voronoiEdge.SetEnd(ToVector3(*edge.vertex1()));
}
}
return voronoiEdge;
}
void AABox::Constrain(CVector3& test_point) const
{
if (test_point.GetX() > mMax.GetX())
test_point.SetX(mMax.GetX());
else if (test_point.GetX() < mMin.GetX())
test_point.SetX(mMin.GetX());
if (test_point.GetZ() > mMax.GetZ())
test_point.SetZ(mMax.GetZ());
else if (test_point.GetZ() < mMin.GetZ())
test_point.SetZ(mMin.GetZ());
if (test_point.GetY() > mMax.GetY())
test_point.SetY(mMax.GetY());
else if (test_point.GetY() < mMin.GetY())
test_point.SetY(mMin.GetY());
}
// --[ Method ]---------------------------------------------------------------
//
// - Class : CMatrix
//
// - prototype : CVector3 operator * (const CVector3& vector)
//
// - Purpose : Transforms a given vector3 using the matrix.
//
// -----------------------------------------------------------------------------
CVector3 CMatrix::operator * (const CVector3& vector) const
{
CVector3 result;
CVector3 v3temp(vector);
result.SetX(m_fM[0][0] * v3temp.X() + m_fM[0][1] * v3temp.Y() + m_fM[0][2] * v3temp.Z() + m_fM[0][3]);
result.SetY(m_fM[1][0] * v3temp.X() + m_fM[1][1] * v3temp.Y() + m_fM[1][2] * v3temp.Z() + m_fM[1][3]);
result.SetZ(m_fM[2][0] * v3temp.X() + m_fM[2][1] * v3temp.Y() + m_fM[2][2] * v3temp.Z() + m_fM[2][3]);
return result;
}
// --[ Method ]---------------------------------------------------------------
//
// - Class : CMatrix
//
// - prototype : CMatrix Inverse()
//
// - Purpose : Returns the inverse transformation matrix.
//
// - Note : IMPORTANT: Algorithm only valid for orthogonal matrices!
//
// -----------------------------------------------------------------------------
CMatrix CMatrix::Inverse() const
{
CMatrix result;
// Transpose rotation submatrix
CVector3 row0(m_fM[0][0], m_fM[1][0], m_fM[2][0]);
CVector3 row1(m_fM[0][1], m_fM[1][1], m_fM[2][1]);
CVector3 row2(m_fM[0][2], m_fM[1][2], m_fM[2][2]);
CVector3 position(m_fM[0][3], m_fM[1][3], m_fM[2][3]);
CVector3 invPosition;
// Solve ecuation system
invPosition.SetX((-row0) * position);
invPosition.SetY((-row1) * position);
invPosition.SetZ((-row2) * position);
// Get scale values
CVector3 scale = Scale();
float sqrSclX = scale.X(); sqrSclX *= sqrSclX;
float sqrSclY = scale.Y(); sqrSclY *= sqrSclY;
float sqrSclZ = scale.Z(); sqrSclZ *= sqrSclZ;
// Shouldn't happen:
assert(!IS_ZERO(sqrSclX));
assert(!IS_ZERO(sqrSclY));
assert(!IS_ZERO(sqrSclZ));
// Normalize axis and multiply by the inverse scale.
row0 = row0 / sqrSclX;
row1 = row1 / sqrSclY;
row2 = row2 / sqrSclZ;
// Insert values
result.SetRow0(row0.X(), row0.Y(), row0.Z(), invPosition.X());
result.SetRow1(row1.X(), row1.Y(), row1.Z(), invPosition.Y());
result.SetRow2(row2.X(), row2.Y(), row2.Z(), invPosition.Z());
result.SetRow3( 0.0f, 0.0f, 0.0f, 1.0f);
return result;
}
