//Separeting Axis Theorem
bool IntersectConvexShape(VertexBuffer* a, ::transform model_a, VertexBuffer* b, ::transform model_b, vec3 &contact_point, vec3 &contact_normal)
{
// potential separating axis
vector<vec3> psa;
vector<vec3> transformed_a;
for(unsigned int i = 0; i < a->GetNum(); i++)
{
vec3 pos = model_a.matrix() * ((Vertex*)a->GetPointer())[i].pos;
transformed_a.push_back(pos);
}
vector<vec3> transformed_b;
for(unsigned int i = 0; i < b->GetNum(); i++)
{
vec3 pos = model_b.matrix() * ((Vertex*)b->GetPointer())[i].pos;
transformed_b.push_back(pos);
}
for(unsigned int i = 0; i < transformed_a.size() - 1; i++)
{
vec3 temp = transformed_a[i+1] - transformed_a[i];
psa.push_back(normalize(vec3(-temp.y, temp.x, 0.0f)));
}
vec3 last = transformed_a[0] - transformed_a[transformed_a.size()-1];
psa.push_back(normalize(vec3(-last.y, last.x, 0.0f)));
for(unsigned int i = 0; i < transformed_b.size()- 1; i++)
{
vec3 temp = transformed_b[i+1] - transformed_b[i];
psa.push_back(normalize(vec3(-temp.y, temp.x, 0.0f)));
}
last = transformed_b[0] - transformed_b[transformed_b.size()-1];
psa.push_back(normalize(vec3(-last.y, last.x, 0.0f)));
//check axies
int min_index = -1;
double min = 0.0;
vec3 min_max_points[4];
for(unsigned int i = 0; i < psa.size(); i++)
{
vec3 r1 = Projection(transformed_a, psa[i], min_max_points[0], min_max_points[1]);
vec3 r2 = Projection(transformed_b, psa[i], min_max_points[2], min_max_points[3]);
if(r1.y < r2.x)
return false;
if(r2.y < r1.x)
return false;
if(r2.x < r1.y && r1.y < r2.y)
{
if((min_index == -1 && r1.y - r2.x > 0) || (min > r1.y - r2.x && r1.y - r2.x > 0))
{
min = r1.y - r2.x;
min_index = i;
}
}
if(r1.x < r2.y && r2.y < r1.y)
{
if((min_index == -1 && r2.y - r1.x < 0) || (min > r2.y - r1.x && r2.y - r1.x < 0))
{
min = r2.y - r1.x;
min_index = i;
}
}
}
if(min_index == -1)
return false;
//result vector, lenght = intersection
float contact_normal_lenght = (float)fabs(min);
contact_normal = -psa[min_index] * contact_normal_lenght;
//initialize
contact_point = (model_a.position + model_b.position) * 0.5f;
bool a_intersect = false;
vector<vec3> *intersected_obj = &transformed_a;
vector<vec3> *intersect_obj = &transformed_b;
if((unsigned int)min_index >= transformed_a.size())
{
a_intersect = true;
intersected_obj = &transformed_b;
intersect_obj = &transformed_a;
min_index -= transformed_a.size();
}
// intersect edge
vec3 p1, p2;
if(min_index == intersected_obj->size() - 1)
{
p1 = (*intersected_obj)[intersected_obj->size()-1];
p2 = (*intersected_obj)[0];
}
else
{
p1 = (*intersected_obj)[min_index];
p2 = (*intersected_obj)[min_index+1];
}
float error = 999999.0f;
// intersect point
p2 = p2 - p1;
double l = length(p2);
for(unsigned int i = 0; i < intersect_obj->size(); i++)
{
vec3 temp = (*intersect_obj)[i];
temp = vec2(temp.x - p1.x, temp.y - p1.y);
double temp_proj = projection(temp, p2);
double dist = fabs(distance_to_line(temp, vec3_zero, p2));
float current_error = (float)fabs(dist - contact_normal_lenght);
float eps = 0.2f;
//if( -eps <= temp_proj && temp_proj <= l+eps && dist <= contact_normal_lenght + eps && dist >= contact_normal_lenght - eps)
if(current_error <= error && -math_epsilon < temp_proj && temp_proj < l + math_epsilon)
{
contact_point = (*intersect_obj)[i];
error = current_error;
}
}
return true;
}
void MVT_3D_Object::Project_to_2D(MVT_2D_Object* ret_object2d, MVT_Viewpoint viewpoint)
{
MVT_Viewpoint_IDX idx_viewpoint = DiscViewpoint_from_ContViewpoint(viewpoint);
cv::Mat mat_P = Projection(viewpoint);
double m_R[3][3] = { {1, 0, 0.5},
{0, -1, 0.5},
{0, 0, 1} };
for(unsigned int pv=0 ; pv<m_num_of_partsNroots ; pv++)
{
ret_object2d->SetOccluded(pv,m_is_occluded[idx_viewpoint.a][idx_viewpoint.e][idx_viewpoint.d][pv]);
/* if( ret_object2d->IsOccluded(pv) )
{
}
else
*/ {
if(
( m_object_category==OBJECT_CATEGORY_CHAIR && pv>10 ) ||
( m_object_category==OBJECT_CATEGORY_TABLE && pv>5 )
)
{
// TODO
// note render.m
}
else
{
cv::Mat mat_R = m_2dparts_front[pv].viewport * cv::Mat(3,3,CV_64F, m_R);
mat_R.at<double>(2,2)=1;
if( pv < m_num_of_parts )
{
unsigned int n_vertices = m_3dparts[pv].vertices.size();
double m[4][n_vertices];
for( unsigned int v=0; v<n_vertices; v++ )
{
m[0][v] = m_3dparts[pv].vertices[v].x;
m[1][v] = m_3dparts[pv].vertices[v].y;
m[2][v] = m_3dparts[pv].vertices[v].z;
m[3][v] = 1;
}
cv::Mat part = mat_P*cv::Mat(4,n_vertices,CV_64F,m);
for( unsigned int v=0; v<n_vertices; v++ )
{
// normalize
double tmp = part.at<double>(3,v);
for( unsigned int r=0; r<4; r++ )
{
part.at<double>(r,v) = part.at<double>(r,v) / tmp;
}
}
double m_vertices2d[3][n_vertices];
for( unsigned int v=0; v<n_vertices; v++ )
{
m_vertices2d[0][v] = part.at<double>(0,v);
m_vertices2d[1][v] = part.at<double>(1,v);
m_vertices2d[2][v] = 1;
}
cv::Mat mat_v = mat_R*cv::Mat(3,n_vertices,CV_64F, m_vertices2d);
double m_center3d[4] = { m_3dparts[pv].center.x,
m_3dparts[pv].center.y,
m_3dparts[pv].center.z,
1 };
cv::Mat center = mat_P*cv::Mat(4,1,CV_64F, m_center3d);
center = center * (1/center.at<double>(3));// normalize
double m_center2d[3] = { center.at<double>(0),
center.at<double>(1),
1 };
cv::Mat mat_c = mat_R*cv::Mat(3,1,CV_64F, m_center2d);
ret_object2d->m_2dparts[pv].vertices.clear();
ret_object2d->m_2dparts[pv].vertices.resize(n_vertices);
for( unsigned int v=0; v<n_vertices; v++ )
{
ret_object2d->m_2dparts[pv].vertices[v].x = mat_v.at<double>(0,v) - mat_c.at<double>(0);
ret_object2d->m_2dparts[pv].vertices[v].y = mat_v.at<double>(1,v) - mat_c.at<double>(1);
}
ret_object2d->m_2dparts[pv].center.x
= mat_c.at<double>(0) - mat_R.at<double>(0,2);
ret_object2d->m_2dparts[pv].center.y
= mat_c.at<double>(1) - mat_R.at<double>(1,2);
}
else
{
int x_max=-INFINITY, x_min=INFINITY, y_max=-INFINITY, y_min=INFINITY;
for( unsigned int p=0; p<m_num_of_parts; p++ )
{
unsigned int n_vertices = ret_object2d->m_2dparts[p].vertices.size();
for( unsigned int v=0; v<n_vertices; v++ )
{
int x = ret_object2d->m_2dparts[p].vertices[v].x + ret_object2d->m_2dparts[p].center.x;
int y = ret_object2d->m_2dparts[p].vertices[v].y + ret_object2d->m_2dparts[p].center.y;
x_max = (x > x_max) ? x : x_max;
x_min = (x < x_min) ? x : x_min;
y_max = (y > y_max) ? y : y_max;
y_min = (y < y_min) ? y : y_min;
}
}
ret_object2d->m_2dparts[pv].vertices.clear();
ret_object2d->m_2dparts[pv].vertices.resize(5);
ret_object2d->m_2dparts[pv].vertices[0] = cv::Point2d(x_min, y_min);
ret_object2d->m_2dparts[pv].vertices[1] = cv::Point2d(x_min, y_max);
ret_object2d->m_2dparts[pv].vertices[2] = cv::Point2d(x_max, y_max);
ret_object2d->m_2dparts[pv].vertices[3] = cv::Point2d(x_max, y_min);
ret_object2d->m_2dparts[pv].vertices[4] = cv::Point2d(x_min, y_min);
ret_object2d->m_2dparts[pv].center.x = 0;
ret_object2d->m_2dparts[pv].center.y = 0;
}
ret_object2d->m_homography_part2front[pv].release();
ret_object2d->m_homography_part2front[pv]
= Homography(ret_object2d->m_2dparts[pv].vertices,m_2dparts_front[pv].vertices);
}
}
}
}
mat4x4 CameraNode::Matrix() const
{
return Projection() * View();
}
//--- Non Standard Singleton Methods
std::shared_ptr<Collision> Collider::IsColliding(Collider* const a_pOther)
{
float dist = glm::distance(GetCenter(), a_pOther->GetCenter());
if (dist > (GetRadius() + a_pOther->GetRadius())) {
return nullptr;
}
if (type != a_pOther->type) {
Collider* box = type == ColliderType::OBB ? this : a_pOther;
Collider* sphere = type == ColliderType::Sphere ? this : a_pOther;
vector3 axes[] = {
box->obb.r,
box->obb.s,
box->obb.t
};
vector3 closestPt = box->obb.c;
vector3 disp = sphere->GetCenter() - box->obb.c;
for (int i = 0; i < 3; i++) {
float dist = glm::dot(disp, glm::normalize(axes[i]));
float extent = glm::length(axes[i]) / 2.0f;
if (dist > extent)
dist = extent;
if (dist < -extent)
dist = -extent;
closestPt += dist * glm::normalize(axes[i]);
}
//MeshManagerSingleton::GetInstance()->AddSphereToQueue(glm::translate(closestPt) * glm::scale(vector3(0.1f)), REYELLOW, SOLID);
vector3 toSphereCenter = closestPt - sphere->GetCenter();
bool colliding = glm::dot(toSphereCenter, toSphereCenter) <= sphere->GetRadius() * sphere->GetRadius();
if (colliding) {
std::shared_ptr<Collision> collision(new Collision());
collision->colliding = true;
vector3 penetration = vector3(0);
if(glm::length2(toSphereCenter) > 0)
penetration = (sphere->GetRadius() - glm::distance(closestPt, sphere->GetCenter())) * glm::normalize(toSphereCenter);
collision->penetrationVector = type == ColliderType::OBB ? penetration : -penetration;
vector3 toSphereEdge = vector3(0);
if (glm::length2(penetration) > 0)
toSphereEdge = glm::normalize(penetration) * sphere->GetRadius();
vector3 sphereEdgePt = sphere->GetCenter() + toSphereEdge;
vector3 obbEdgePt = closestPt;
collision->intersectPoint1 = type == ColliderType::OBB ? obbEdgePt : sphereEdgePt;
collision->intersectPoint2 = type == ColliderType::OBB ? sphereEdgePt : obbEdgePt;
return collision;
}
return nullptr;
}
//If they are both circles, we have already checked their radii
else if (type == ColliderType::Sphere) {
std::shared_ptr<Collision> collision(new Collision());
collision->colliding = true;
return collision;
}
else {
vector3 r1 = obb.r;
vector3 s1 = obb.s;
vector3 t1 = obb.t;
OBB obb2 = a_pOther->obb;
vector3 r2 = obb2.r;
vector3 s2 = obb2.s;
vector3 t2 = obb2.t;
vector3 axes[] = {
//Normals of OBB1
glm::cross(r1, s1),
glm::cross(r1, t1),
glm::cross(s1, t1),
//Normals of OBB2
glm::cross(r2, s2),
glm::cross(r2, t2),
glm::cross(s2, t2),
//Normals between OBB1 & 2
glm::cross(r1, r2),
glm::cross(r1, s2),
glm::cross(r1, t2),
glm::cross(s1, r2),
glm::cross(s1, s2),
glm::cross(s1, t2),
glm::cross(t1, r2),
glm::cross(t1, s2),
glm::cross(t1, t2),
};
for (int i = 0; i < 15; i++) {
if (glm::length(axes[i]) == 0)
continue;
Projection proj1 = Projection(obb.GetWorldVerts(), axes[i]);
Projection proj2 = Projection(a_pOther->obb.GetWorldVerts(), axes[i]);
lastCollision = (obb.c + a_pOther->obb.c) / 2.0f;
if (!proj1.Overlaps(proj2)) {
return nullptr;
}
}
std::shared_ptr<Collision> collision(new Collision());
//TODO: SAT penetration vector
collision->colliding = true;
return collision;
}
}
int FluidSolver::Solve(float time)
{
// Propagate the solution until requested time is reached
int iterations = 0;
for (float elapsed = 0; elapsed < time;) {
// Determine timestep for stability
float dt = ComputeTimestep();
std::cerr << "Propagating solution with dt = " << dt << std::endl;
if (dt > time-elapsed)
dt = time-elapsed;
elapsed += dt;
// Compute current volume
mCurrentVolume = 0;
std::set<LevelSet *>::const_iterator iter = mFluids.begin();
std::set<LevelSet *>::const_iterator iend = mFluids.end();
while (iter != iend) {
mCurrentVolume += (*iter)->ComputeVolume((*iter)->GetDx());
iter++;
}
std::cout << "Current volume: " << mCurrentVolume << std::endl;
std::cout << "Initial volume: " << mInitialVolume << std::endl;
std::cout << "Loss of volume: " << mInitialVolume - mCurrentVolume << std::endl;
// Classify all voxels as either solid, fluid or empty
std::cerr << "Classifying voxels..." << std::endl;
ClassifyVoxels();
// Self advection
std::cerr << "Self advection..." << std::endl;
SelfAdvection(dt, 6);
// Add the external forces
std::cerr << "Adding external forces..." << std::endl;
ExternalForces(dt);
// Add the friction/diffusion forces
std::cerr << "Adding diffusion forces..." << std::endl;
const float amount = 8;
//Diffusion(amount,dt,0);
//Diffusion(amount,dt,1);
//Diffusion(amount,dt,2);
// Enforce boundary conditions
std::cerr << "Boundary conditions..." << std::endl;
EnforceDirichletBoundaryCondition();
// Compute the projection for preserving volume
std::cerr << "Projection..." << std::endl;
Projection();
// The projection should not violate the boundary conditions,
// but it might becacuse of numerical errors (the solution is not
// exact). Enforce the boundary conditions again to safeguard
// against this.
std::cerr << "Boundary conditions..." << std::endl;
EnforceDirichletBoundaryCondition();
// Extend the velocites to "air" so the entire level set narrowband
// is advected by the velocity field
VelocityExtension();
std::cerr << "Done with iteration" << std::endl;
iterations++;
}
return iterations;
}
const Conserved SinkFlux::calcHydroFlux
(const Tessellation& tess,
const vector<Vector2D>& point_velocities,
const vector<ComputationalCell>& cells,
const EquationOfState& eos,
const size_t i) const
{
const Edge& edge = tess.GetEdge(static_cast<int>(i));
const std::pair<bool,bool> flags
(edge.neighbors.first>=0 && edge.neighbors.first<tess.GetPointNo(),
edge.neighbors.second>=0 && edge.neighbors.second<tess.GetPointNo());
assert(flags.first || flags.second);
if(!flags.first){
const size_t right_index =
static_cast<size_t>(edge.neighbors.second);
const ComputationalCell& right_cell = cells[right_index];
if(right_cell.stickers.find("dummy")->second)
return Conserved();
const Vector2D p = Parallel(edge);
const Primitive right = convert_to_primitive(right_cell,eos);
// const Primitive left = reflect(right,p);
const Primitive left = right;
const Vector2D n = remove_parallel_component
(tess.GetMeshPoint(edge.neighbors.second) -
edge.vertices.first, p);
return rotate_solve_rotate_back
(rs_, left, right, 0, n, p);
}
if(!flags.second){
const size_t left_index =
static_cast<size_t>(edge.neighbors.first);
const ComputationalCell& left_cell = cells[left_index];
if(left_cell.stickers.find("dummy")->second)
return Conserved();
const Primitive left = convert_to_primitive(left_cell, eos);
const Vector2D p = Parallel(edge);
// const Primitive right = reflect(left,p);
const Primitive right = left;
const Vector2D n = remove_parallel_component
(edge.vertices.second -
tess.GetMeshPoint(edge.neighbors.first), p);
return rotate_solve_rotate_back
(rs_, left, right, 0, n, p);
}
const size_t left_index =
static_cast<size_t>(edge.neighbors.first);
const size_t right_index =
static_cast<size_t>(edge.neighbors.second);
const ComputationalCell& left_cell = cells[left_index];
const ComputationalCell& right_cell = cells[right_index];
if(left_cell.stickers.find("dummy")->second &&
right_cell.stickers.find("dummy")->second)
return Conserved();
const Vector2D p = Parallel(edge);
const Vector2D n =
tess.GetMeshPoint(edge.neighbors.second) -
tess.GetMeshPoint(edge.neighbors.first);
const double velocity = Projection
(tess.CalcFaceVelocity
(point_velocities[left_index],
point_velocities[right_index],
tess.GetCellCM(edge.neighbors.first),
tess.GetCellCM(edge.neighbors.second),
calc_centroid(edge)),n);
if(left_cell.stickers.find("dummy")->second){
const Primitive right =
convert_to_primitive(right_cell, eos);
ComputationalCell ghost;
ghost.density = right.Density/100;
ghost.pressure = right.Pressure/100;
ghost.velocity = Vector2D(0,0);
const Primitive left = convert_to_primitive(ghost,eos);
/*
ScalarProd(n,right.Velocity) < 0 ? right :
reflect(right,p);
*/
return rotate_solve_rotate_back
(rs_,left,right,velocity,n,p);
}
if(right_cell.stickers.find("dummy")->second){
const Primitive left =
convert_to_primitive(left_cell, eos);
ComputationalCell ghost;
ghost.density = left.Density/100;
ghost.pressure = left.Pressure/100;
ghost.velocity = Vector2D(0,0);
const Primitive right = convert_to_primitive(ghost,eos);
/*
ScalarProd(n,left.Velocity)>0 ?
left : reflect(left,p);
*/
return rotate_solve_rotate_back
(rs_,left,right,velocity,n,p);
}
const Primitive left =
convert_to_primitive(left_cell, eos);
const Primitive right =
convert_to_primitive(right_cell, eos);
return rotate_solve_rotate_back
(rs_,left,right,velocity,n,p);
}
inline _VectorType Rejection(const _VectorType& v, const _VectorType& w) {
return v - Projection(v,w);
}
bool CollisionManager::DetectCollision(BouncyThing &shape1, BouncyThing &shape2) {
std::vector<sf::Vector2f> axis = shape1.getAxes();
sf::Vector2f smallestAxis;
double smallestOverlap = 100000000;
bool shape1SA = false;
bool shape2SA = false;
for (int i = 0; i < shape1.getAxes().size(); i++) {
sf::Vector2f p = Projection(shape1,axis[i]);
sf::Vector2f p2 = Projection(shape2,axis[i]);
//checking if there is a gap between the shapes on the project axis
if (p.y < p2.x || p.x > p2.y)
return false;
//getting the smallest overlap value between the two shapes (MTV)
if (p.y > p2.y) {
if ((p2.y - p.x) < smallestOverlap) {
smallestOverlap = p2.y - p.x;
smallestAxis = axis[i];
shape1SA = true;
}
}
else {
if ((p.y - p2.x) < smallestOverlap) {
smallestOverlap =p.y - p2.x;
smallestAxis = axis[i];
shape1SA = true;
}
}
}
axis = shape2.getAxes();
for (int i = 0; i < shape2.getAxes().size(); i++) {
sf::Vector2f p = Projection(shape2,axis[i]);
sf::Vector2f p2 = Projection(shape1,axis[i]);
//checking if there is a gap between the shapes on the projection axis
if (p.y < p2.x || p.x > p2.y)
return false;
//getting the smallest overlap value between the two shapes (MTV)
if (p.y > p2.y) {
if ((p2.y - p.x) < smallestOverlap) {
smallestOverlap = p2.y - p.x;
smallestAxis = axis[i];
shape1SA = false;
shape2SA = true;
}
}
else {
if ((p.y - p2.x) < smallestOverlap) {
smallestOverlap =p.y - p2.x;
smallestAxis = axis[i];
shape1SA = false;
shape2SA = true;
}
}
}
//if we get to this point, then every axis had an overlap-----------------------------------------------------------------------------------------
//if we get to this point, then every axis had an overlap-----------------------------------------------------------------------------------------
//getting the minimum translation vector and applying it to the two shapes to seperate them.
smallestAxis.x = (smallestAxis.x * smallestOverlap) /2;
smallestAxis.y = (smallestAxis.y * smallestOverlap) /2;
if (shape1SA = true) {
shape1.applyMove(smallestAxis);
shape2.applyMove(-smallestAxis);
}
else if (shape2SA = true) {
shape2.applyMove(smallestAxis);
shape1.applyMove(-smallestAxis);
}
//reflecting the two shapes vectors using the normal between the two mid points of the shapes
sf::Vector2f normal = shape1.shape.getPosition() - shape2.shape.getPosition();
normal = normal / sqrt((normal.x * normal.x) + (normal.y * normal.y));
sf::Vector2f outVect1 = shape1.velocity - (((DotProduct(normal, shape1.velocity) * normal)) + ((DotProduct(normal, shape1.velocity) * normal)));
normal = -normal;
sf::Vector2f outVect2 = shape2.velocity - (((DotProduct(normal, shape2.velocity) * normal)) + ((DotProduct(normal, shape2.velocity) * normal)));
shape1.velocity = outVect1;
shape2.velocity = outVect2;
return true;
}
void Camera::RecalculateMatrixStack ( )
{
m_view = View( m_position, m_target - m_position, float3(0.0f, 1.0f, 0.0f));
m_projection = Projection( m_verticalFov, m_aspectRatio, m_nearPlane, m_farPlane );
m_viewProjection = m_view * m_projection;
}
void MarbleWidget::setProjection( int projection )
{
setProjection( Projection( qAbs( projection ) % (Mercator+1) ) );
}