// 加载椭球面。
// radiusX、radiusY、radiusZ为各轴半轴长，必须为正。
// angleBeginXY和angleEndXY为与XY平面的开始、结束角，[-M_PI/2,M_PI/2]。
// angleBeginXY > angleEndXY时，加载椭球外表面。
// angleBeginXY < angleEndXY时，加载椭球内表面。
// angleBeginXY和angleEndXY不可以相等。
// angleBeginZ和angleEndZ为绕Z轴的开始、结束角，逆时针，可以跨过0，不可以相等 。
bool GlGrid::LoadSphere(float radiusX,float radiusY,float radiusZ,
float angleBeginXY,float angleEndXY,float angleBeginZ,float angleEndZ)
{
if(radiusX <= 0.0f || radiusY <= 0.0f || radiusZ <= 0.0f)
return false;
if(m_row < 3 || m_col < 4)
return false;
//
if(angleBeginXY < -M_PI/2-TOLERANCE || angleBeginXY > M_PI/2+TOLERANCE
|| angleEndXY < -M_PI/2-TOLERANCE || angleEndXY > M_PI/2+TOLERANCE)
return false;
//
while(angleBeginZ > 2*M_PI+TOLERANCE)
angleBeginZ -= 2*M_PI;
while(angleBeginZ < 0)
angleBeginZ += 2*M_PI;
while(angleEndZ > 2*M_PI+TOLERANCE)
angleEndZ -= 2*M_PI;
while(angleEndZ < 0)
angleEndZ += 2*M_PI;
//
// 保证angleEndZ在angleBeginZ前面。
if(angleEndZ < angleBeginZ)
angleEndZ += 2*M_PI;
//
// 开始。
float angXY = angleBeginXY;
float angXYdelta = (angleEndXY - angleBeginXY) / (m_row - 1);
for(int i=0;i<m_row;i++,angXY+=angXYdelta)
{
// 设置Z。
float z = radiusZ * sinf(angXY);
for(int j=0;j<m_col;j++)
m_vertex[(i*m_col+j) * 3 + 2] = z;
//
// XY。
float tmp = cosf(angXY);
float rX = radiusX * tmp;
float rY = radiusY * tmp;
//
float angZ = angleBeginZ;
float angZdelta = (angleEndZ - angleBeginZ) / (m_col - 1);
for(int j=0;j<m_col;j++,angZ+=angZdelta)
{
m_vertex[(i*m_col+j) * 3 ] = rX * cosf(angZ);
m_vertex[(i*m_col+j) * 3 + 1] = rY * sinf(angZ);
}
}
//
// 计算点法向。
if(angleEndZ-angleBeginZ < 2*M_PI-TOLERANCE)
calculateNormal(false);
else
calculateNormal(true);
//
return true;
}
// TODO: Is there a better way to draw stuff
void ofxGtsSurface::draw(DrawType type) {
if(!loaded) {
//ofLog(OF_LOG_NOTICE, "Gts surface not loaded");
return;
}
GtsFace * first = NULL;
// TODO: Do we really need to do this just to get the first face?
gts_surface_foreach_face (surface, (GtsFunc) pick_first_face, &first);
if (first) {
GtsSurfaceTraverse * t = gts_surface_traverse_new (surface, first);
GtsFace * f;
guint level;
glBegin((type == TRIANGLES) ? GL_TRIANGLE_STRIP : GL_LINES);
while ((f = gts_surface_traverse_next (t, &level))) {
// Edge 1
GtsPoint p1 = f->triangle.e1->segment.v1->p;
GtsPoint p2 = f->triangle.e1->segment.v2->p;
// Edge 2
GtsPoint p3 = f->triangle.e2->segment.v1->p;
GtsPoint p4 = f->triangle.e2->segment.v2->p;
// Edge 3
GtsPoint p5 = f->triangle.e3->segment.v1->p;
GtsPoint p6 = f->triangle.e3->segment.v2->p;
// TODO: Figure out what is going on with the normals
if(type==TRIANGLES) {
ofPoint normal = calculateNormal(p1, p2, p3);
glNormal3f(normal.x, normal.y, normal.z);
}
glVertex3f(p1.x, p1.y, p1.z);
glVertex3f(p2.x, p2.y, p2.z);
glVertex3f(p3.x, p3.y, p3.z);
if(type==TRIANGLES) {
ofPoint normal = calculateNormal(p4, p5, p6);
glNormal3f(normal.x, normal.y, normal.z);
}
glVertex3f(p4.x, p4.y, p4.z);
glVertex3f(p5.x, p5.y, p5.z);
glVertex3f(p6.x, p6.y, p6.z);
}
glEnd();
gts_surface_traverse_destroy (t);
}
}
void Phy::resolveCollisionBoundary(ContactBoundary* c) {
Circle* circle = c->circle;
Box* box = c->box;
Vec2 direction;
if (circle->velocity.equal(0, 0)) {
direction = c->normal.normalise().scale(-1);
}
else {
direction = circle->velocity.normalise();
}
Vec2 temp;
while (absf(c->penetration) > 1) {
if (!direction.equal(0, 0)) {
Vec2 error = direction.scale(c->penetration * -1);
circle->position = vecSum(circle->position, error);
}
c->normal = calculateNormal(circle, box, &temp);
c->penetration = circle->radius - sqrt(c->normal.lengthSquared());
}
c->normal = c->normal.normalise();
circle->velocity = vecSum(c->normal.scale(vecDot(c->normal, circle->velocity) * -2), circle->velocity);
}
IFloatBuffer* NormalsUtils::createTriangleSmoothNormals(const IFloatBuffer* vertices,
const IShortBuffer* indices) {
const int verticesSize = vertices->size();
IFloatBuffer* normals = IFactory::instance()->createFloatBuffer(verticesSize);
for (int i = 0; i < verticesSize; i++) {
normals->rawPut(i, 0);
}
const int indicesSize = indices->size();
for (int i = 0; i < indicesSize; i += 3) {
const short index0 = indices->get(i);
const short index1 = indices->get(i + 1);
const short index2 = indices->get(i + 2);
const Vector3F normal = calculateNormal(vertices, index0, index1, index2);
addNormal(normals, index0, normal);
addNormal(normals, index1, normal);
addNormal(normals, index2, normal);
}
for (int i = 0; i < verticesSize; i += 3) {
const float x = normals->get(i);
const float y = normals->get(i + 1);
const float z = normals->get(i + 2);
const Vector3F normal = Vector3F(x, y, z).normalized();
normals->rawPut(i , normal._x);
normals->rawPut(i + 1, normal._y);
normals->rawPut(i + 2, normal._z);
}
return normals;
}
void TriMesh::computeNormals() {
// skip
// Calculate triangle normals
for(int i = 0; i < m_triangles.size(); i++){
glm::vec3 v1; glm::vec3 v2; glm::vec3 v3;
HalfEdge* tempLeader = m_triangles[i]->getLeadingHalfEdge();
v1 = tempLeader->getSourceNode()->m_pos_;
tempLeader = tempLeader->getNext();
v2 = tempLeader->getSourceNode()->m_pos_;
tempLeader = tempLeader->getNext();
v3 = tempLeader->getSourceNode()->m_pos_;
m_triangles[i]->m_N_ = calculateNormal(v1,v2,v3);
}
// Calculate vertex normals by averaging triange normals
for(int i = 0; i < m_nodes.size(); i++){
Node n = m_nodes[i];
HalfEdge* startEdge = n.getLeadingHalfEdge();
Triangle* startTriangle = startEdge->getTriangle();
glm::vec3 accNormal = startTriangle->m_N_;
int triangleCount = 1;
for(HalfEdge* tempEdge = startEdge->getVtxRingNext(); tempEdge != startEdge; tempEdge = tempEdge->getVtxRingNext()){
if (tempEdge == NULL)
break;
accNormal += tempEdge->getTriangle()->m_N_;
triangleCount++;
}
glm::vec3 count = glm::vec3(triangleCount);
m_nodes[i].m_N_ = accNormal/count;
}
// unskip
}
std::shared_ptr< const CViewSegment2D > CViewSegment2D::getNormal() {
if ( !m_NormalCalculated ) {
calculateNormal();
m_NormalCalculated = true;
}
return m_Normal;
}
bool Phy::AABBvsCircle(Contact *c) {
Circle* circle = c->circle;
Box* box = &c->axis.player[c->player];
box->position = Vec2(c->axis.position.x + c->axis.x_offset, c->axis.position.y + c->axis.player[c->player].y_offset);
bool collide = false;
//Closest point on Box to the center of Circle - only for debug
Vec2 closest;
Vec2 normal = calculateNormal(circle, box, &closest);
if (normal.lengthSquared() < circle->radius * circle->radius) {
collide = true;
//std::cout << "contact" << "\t" <<
c->penetration = circle->radius - sqrt(normal.lengthSquared());
//<< "\n";
c->normal = normal;
}
return collide;
}
void Triangle::copyTriangle(Vec3<float> top, Vec3<float> right, Vec3<float> left)
{
corners[0] = top;
corners[1] = right;
corners[2] = left;
normal = calculateNormal();
}
float StlSphere::getArea(Facet *facet)
{
float cross[3][3];
float sum[3];
//.
float n[3];
for (int i = 0; i < 3; i++) {
cross[i][0] = ((facet->vector[i].y * facet->vector[(i + 1) % 3].z) -
(facet->vector[i].z * facet->vector[(i + 1) % 3].y));
cross[i][1] = ((facet->vector[i].z * facet->vector[(i + 1) % 3].x) -
(facet->vector[i].x * facet->vector[(i + 1) % 3].z));
cross[i][2] = ((facet->vector[i].x * facet->vector[(i + 1) % 3].y) -
(facet->vector[i].y * facet->vector[(i + 1) % 3].x));
}
sum[0] = cross[0][0] + cross[1][0] + cross[2][0];
sum[1] = cross[0][1] + cross[1][1] + cross[2][1];
sum[2] = cross[0][2] + cross[1][2] + cross[2][2];
// This should already be done. But just in case, let's do it again
//calculateNormal(n, facet);
//normalizeVector(n);
//float area = 0.5 * (n[0] * sum[0] + n[1] * sum[1] + n[2] * sum[2]);
calculateNormal( facet);
normalizeVector(&facet->normal);
float area = 0.5 * (facet->normal.x * sum[0] + facet->normal.y * sum[1] + facet->normal.z * sum[2]);
return area;
}
void TileBound::flip()
{
QPointF temp = mLeft;
mLeft = mRight;
mRight = temp;
calculateNormal();
}
// 把网格变成带缺口的圆台侧面。Z轴为母线。
// height为高度，实数范围，正就在Z正方向，负就在Z负方向，0就在XOY平面。
// radiusBottom为底半径,radiusTop为顶半径，必须非负。
// angle为角度，0~2*M_PI，截取；
// angleBegin为开始角度，绕Z轴，从X轴起算逆时针。
bool GlGrid::loadCylinder(float height, float radiusBottom, float radiusTop, float angle, float angleBegin)
{
if(!isValid())
return false;
if(radiusBottom < 0.0f || radiusTop < 0.0f)
return false;
if(m_row < 2 || m_col < 4)
return false;
//
while(angleBegin > 2*M_PI)
angleBegin -= 2*M_PI;
while(angleBegin < 0)
angleBegin += 2*M_PI;
if(angle > 2*M_PI)
angle = 2*M_PI;
if(angle < 0.0f)
angle = 0.0f;
//
// 设置Z。
float heiDelta = height / (m_row-1);
float hei = 0.0f;
for(int i=0;i<m_row;i++,hei+=heiDelta)
for(int j=0;j<m_col;j++)
m_vertex[(i*m_col+j) * 3 + 2] = hei;
//
// 设置XY。
float ang = angleBegin;
float angDelta = angle / (m_col-1);
for(int j=0;j<m_col;j++,ang+=angDelta)
{
float rad = radiusBottom;
float radDelta = (radiusTop - radiusBottom) / (m_row - 1);
for(int i=0;i<m_row;i++,rad+=radDelta)
{
m_vertex[(i*m_col+j) * 3 ] = cosf(ang) * rad;
m_vertex[(i*m_col+j) * 3 + 1] = sinf(ang) * rad;
}
}
//
// 计算点法向。
if(angle < 2*M_PI-TOLERANCE)
calculateNormal(false);
else
calculateNormal(true);
//
return true;
}
bool KRTriangle3::sphereCast(const KRVector3 &start, const KRVector3 &dir, float radius, KRVector3 &hit_point, float &hit_distance) const
{
// Dir must be normalized
const float SMALL_NUM = 0.001f; // anything that avoids division overflow
KRVector3 tri_normal = calculateNormal();
float d = KRVector3::Dot(tri_normal, m_c[0]);
float e = KRVector3::Dot(tri_normal, start) - radius;
float cotangent_distance = e - d;
KRVector3 plane_intersect;
float plane_intersect_distance;
float denom = KRVector3::Dot(tri_normal, dir);
if(denom > -SMALL_NUM) {
return false; // dir is co-planar with the triangle or going in the direction of the normal; no intersection
}
// Detect an embedded plane, caused by a sphere that is already intersecting the plane.
if(cotangent_distance <= 0 && cotangent_distance >= -radius * 2.0f) {
// Embedded plane - Sphere is already intersecting the plane.
// Use the point closest to the origin of the sphere as the intersection
plane_intersect = start - tri_normal * (cotangent_distance + radius);
plane_intersect_distance = 0.0f;
} else {
// Sphere is not intersecting the plane
// Determine the first point hit by the swept sphere on the triangle's plane
plane_intersect_distance = -(cotangent_distance / denom);
plane_intersect = start + dir * plane_intersect_distance - tri_normal * radius;
}
if(plane_intersect_distance < 0.0f) {
return false;
}
if(containsPoint(plane_intersect)) {
// Triangle contains point
hit_point = plane_intersect;
hit_distance = plane_intersect_distance;
return true;
} else {
// Triangle does not contain point, cast ray back to sphere from closest point on triangle edge or vertice
KRVector3 closest_point = closestPointOnTriangle(plane_intersect);
float reverse_hit_distance;
if(_intersectSphere(closest_point, -dir, start, radius, reverse_hit_distance)) {
// Reverse cast hit sphere
hit_distance = reverse_hit_distance;
hit_point = closest_point;
return true;
} else {
// Reverse cast did not hit sphere
return false;
}
}
}
Face :: Face(Vertex* v1, Vertex* v2, Vertex* v3)
{
vertexVector.push_back(v1);
vertexVector.push_back(v2);
vertexVector.push_back(v3);
normal = calculateNormal();
center = calculateCenter();
}
void Face :: setVertices(Vertex* v1, Vertex* v2, Vertex* v3)
{
vertexVector[0] = v1;
vertexVector[1] = v2;
vertexVector[2] = v3;
normal = calculateNormal();
center = calculateCenter();
}
/*
* update visibility of faces and their normals
*/
void testApp::updateVisibility(){
for(int x=0; x < mapWidth; x++){
for(int y=0; y < mapHeight; y++){
for(int z=0; z < mapDepth; z++){
Block* block = &array3D[x][y][z];
int vmask = 0;
if(x <= 0 || x >= mapWidth - 1 || y >= mapHeight - 1 || z <= 0 || z >= mapDepth - 1){
block->visMask = 63;
for(int i = 0; i < 6; i++){
calculateNormal(block, i);
}
} else if(y <= 0){
block->visMask = 0;
} else {
vmask = 0;
BlockType blockTypes[2] = {NONE, SNOW};
for(int i = 0; i < 2; i++){
if(array3D[x - 1][y][z].type == blockTypes[i]){
vmask |= VIS_LEFT;
calculateNormal(block, 3);
}
if(array3D[x + 1][y][z].type == blockTypes[i]){
vmask |= VIS_RIGHT;
calculateNormal(block, 2);
}
if(array3D[x][y - 1][z].type == blockTypes[i] ){
vmask |= VIS_BOTTOM;
calculateNormal(block, 4);
}
if (array3D[x][y + 1][z].type == blockTypes[i]){
vmask |= VIS_TOP;
calculateNormal(block, 0);
}
if(array3D[x][y][z - 1].type == blockTypes[i] ){
vmask |= VIS_BACK;
calculateNormal(block, 5);
}
if (array3D[x][y][z + 1].type == blockTypes[i]){
vmask |= VIS_FRONT;
calculateNormal(block, 1);
}
}
block->visMask = vmask;
//set the normal for this face
}
}
}
}
}
/**
* Sets vertexes of triangle.
* @param v1
* @param v2
* @param v3
*/
void GlTriangle::setTriangle(GlVertex* v1, GlVertex* v2, GlVertex* v3){
//if(vx_ != NULL) delete vx_; // prevents memory leak
v1_ = v1;
//if(vy_ != NULL) delete vy_;
v2_ = v2;
//if(vz_ != NULL) delete vz_;
v3_ = v3;
calculateNormal();
}
void ofxFace::addVertex(ofxVertex *vertex)
{
vertex->addFace(this);
verts.push_back(vertex);
if(verts.size() == 3)
{
calculateCentroid();
calculateNormal();
}
}
IFloatBuffer* NormalsUtils::createTriangleStripSmoothNormals(const IFloatBuffer* vertices,
const IShortBuffer* indices) {
const int verticesSize = vertices->size();
IFloatBuffer* normals = IFactory::instance()->createFloatBuffer(verticesSize);
for (int i = 0; i < verticesSize; i++) {
normals->rawPut(i, 0);
}
short index0 = indices->get(0);
short index1 = indices->get(1);
const int indicesSize = indices->size();
for (int i = 2; i < indicesSize; i++) {
const short index2 = indices->get(i);
const Vector3F normal = (i % 2 == 0)
/* */ ? calculateNormal(vertices, index0, index1, index2)
/* */ : calculateNormal(vertices, index0, index2, index1);
addNormal(normals, index0, normal);
addNormal(normals, index1, normal);
addNormal(normals, index2, normal);
index0 = index1;
index1 = index2;
}
//http://stackoverflow.com/questions/3485034/convert-triangle-strips-to-triangles
for (int i = 0; i < verticesSize; i += 3) {
const float x = normals->get(i);
const float y = normals->get(i + 1);
const float z = normals->get(i + 2);
const Vector3F normal = Vector3F(x, y, z).normalized();
normals->rawPut(i , normal._x);
normals->rawPut(i + 1, normal._y);
normals->rawPut(i + 2, normal._z);
}
return normals;
}
//SET
void polygon::setPoints( const vertex* points, const int pointsCount )
{
this->pointsCount = pointsCount;
this->points = new vertex[ pointsCount ];
for( int i = 0; i < pointsCount; i++ )
{
this->points[i].wx = points[i].wx;
this->points[i].wy = points[i].wy;
this->points[i].wz = points[i].wz;
}
calculateNormal( this->points[1], this->points[2] );
}
/**
* Genera las normales de los triángulos superficie del terreno
*/
void genNormalesT(){
int n2 = 2*(ANCHO-1);
int ind1, ind2;
for (int i=0; i<(ALTO-1)*(ANCHO-1)*2; i+=2){ // PARES
// 0 (0 2n+0) = (0,0)(1,1)(1,0)
// 12(1 2n+4) = (1,2)(2,3)(2,2)
// 14(1 2n+6) = (1,3)(2,4)(2,3)
ind1 = i/(n2);
ind2 = (i%(n2))/2;
GLfloat* v0 = vertices[ind1][ind2];
GLfloat* v1 = vertices[ind1+1][ind2+1];
GLfloat* v2 = vertices[ind1+1][ind2];
GLfloat* normal = calculateNormal(v0,v1,v2);
normalest[i][0] = normal[0];
normalest[i][1] = normal[1];
normalest[i][2] = normal[2];
delete normal;
}
for (int i=1; i<(ALTO-1)*(ANCHO-1)*2; i+=2){ // IMPARES
// 13(1 2n+5) = (1,2)(1,3)(2,3)
// 15(1 2n+7) = (1,3)(1,4)(2,4)
ind1 = i/(n2);
ind2 = (i%(n2))/2;
GLfloat* v0 = vertices[ind1][ind2];
GLfloat* v1 = vertices[ind1][ind2+1];
GLfloat* v2 = vertices[ind1+1][ind2+1];
GLfloat* normal = calculateNormal(v0,v1,v2);
normalest[i][0] = normal[0];
normalest[i][1] = normal[1];
normalest[i][2] = normal[2];
delete normal;
}
}
void Ptriangle::calcValues(){
float a = sqrtf( pow((x1 - x3),2) + pow((y1 - y3),2) + pow((z1 - z3),2));
float b = sqrtf( pow((x2 - x1),2) + pow((y2 - y1),2) + pow((z2 - z1),2));
float c = sqrtf( pow((x3 - x2),2) + pow((y3 - y2),2) + pow((z3 - z2),2));
float cosy=(pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b);
float cosa=(-pow(a,2)+pow(b,2)+pow(c,2))/(2*b*c);
float cosb=(pow(a,2)-pow(b,2)+pow(c,2))/(2*a*c);
p0[0]=c-a*cosb;
p0[1]=a*sqrt(1-pow(cosb,2));
p1[0]=0;
p1[1]=0;
p2[0]=c;
p2[1]=0;
calculateNormal();
}
RawModel Terrain::generateTerrain(Loader &loader, std::string heightMapFile){
RawImageData heightMap = loader.loadRawImageData(heightMapFile);
processHeightMap(heightMap);
vertex_count = heightMap.getHeight();
heights.resize(vertex_count, std::vector<float>(vertex_count, 0));
std::vector<float> normals;
std::vector<float> textureCoords;
std::vector<int> indices;
for (int i = 0; i < vertex_count; i++) {
for (int j = 0; j < vertex_count; j++){
vertices.push_back((float)j / ((float)vertex_count - 1) * size);
float height = getHeight(j, i);
heights[j][i] = height;
vertices.push_back( height );
vertices.push_back((float)i / ((float)vertex_count - 1) * size);
glm::vec3 normal = calculateNormal(j, i);
normals.push_back( normal.x );
normals.push_back( normal.y );
normals.push_back( normal.z );
textureCoords.push_back((float)j / ((float)vertex_count - 1));
textureCoords.push_back((float)i / ((float)vertex_count - 1));
}
}
for (int gz = 0; gz < vertex_count - 1; gz++){
for (int gx = 0; gx < vertex_count - 1; gx++){
int topLeft = (gz*vertex_count) + gx;
int topRight = topLeft + 1;
int bottomLeft = ((gz + 1)*vertex_count) + gx;
int bottomRight = bottomLeft + 1;
indices.push_back( topLeft );
indices.push_back( bottomLeft );
indices.push_back( topRight );
indices.push_back( topRight );
indices.push_back( bottomLeft );
indices.push_back( bottomRight );
}
}
return loader.loadToVao(vertices, textureCoords, normals, indices);
}
Terrain::Terrain(std::string filename, float resolution, float interval)
{
// set member values
m_interval = interval;
m_resolution = resolution;
// initialize the height field
loadHeightmap(filename);
for (int z = 1; z < m_resolutionY - 1; z++){
for (int x = 1; x < m_resolutionX - 1; x++){
m_normals.push_back(calculateNormal(x, z));
m_vertices.push_back(glm::vec4(x, getHeight(glm::vec2(x, z)), z, 1.0));
//m_uvs.push_back(glm::vec2(x / (float)m_resolutionX, z / (float)m_resolutionY));
m_uvs.push_back(glm::vec2((float)x / 5, (float)z / 5));
}
}
int offset = 0;
for (int z = 0; z < m_resolutionY - 3; z++){
for (int x = 0; x < m_resolutionX - 3; x++){
// 1. Triangle
m_index.push_back(offset + x);
m_index.push_back(offset + x + 1);
m_index.push_back(offset + x + (m_resolutionY - 2));
// 2. Triangle
m_index.push_back(offset + x + (m_resolutionY - 2));
m_index.push_back(offset + x + 1);
m_index.push_back(offset + x + (m_resolutionY - 2) + 1);
}
offset += (m_resolutionY - 2);
}
m_indices = m_index.size();
m_points = m_vertices.size();
setNormalsTrue();
setIndexTrue();
setUVTrue();
}
//Anzeigefunktionen ursprünglich
void polygon::display()
{
//vor jeder anzeige, die normalen neu berechen:
vertex v1 = VectCreate( this->points[0], this->points[1] );
vertex v2 = VectCreate( this->points[0], this->points[2] );
calculateNormal( v1, v2 );
//TEST: NORMALE ANZEIGEN
// drawNormal();
//weil schön ist: MESH anzeigen
drawMesh();
//normale angeben
glNormal3d( this->normal.wx, this->normal.wy, this->normal.wz );
//textur festlegen
glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, this->texture.xsize, this->texture.ysize, 0, GL_RGBA, GL_UNSIGNED_BYTE, this->texture.content ); //Aussehen der Textur festlegen
// Farbe des Polygons festlegen
glColor3d( this->v_RGBcolor->wx, this->v_RGBcolor->wy, this->v_RGBcolor->wz );
// glColor3d( this->helligkeit, this->helligkeit, this->helligkeit ); //Helligkeitswert finden
//glBegin( GL_TRIANGLE_FAN );
glBegin( GL_POLYGON );
for( long i = 0; i < this->pointsCount; i++ )
{
//texturCoordinaten in 2d
// vertex texCoords = this->textureCoords[ i ];
// glTexCoord2d( texCoords.wx, texCoords.wy );
//position in 3D
vertex pos = this->points[ i ];
glVertex3d( pos.wx, pos.wy, pos.wz );
}
glEnd();
}
bool Phy::boundaryVsCircle(ContactBoundary *c) {
Circle* circle = c->circle;
Box* box = c->box;
bool collide = false;
//Closest point on Box to the center of Circle
Vec2 closest;
Vec2 normal = calculateNormal(circle, box, &closest);
if (normal.lengthSquared() < circle->radius * circle->radius) {
collide = true;
//std::cout << "contact" << "\t" <<
//std::cout << "BOOOMM\n";
c->penetration = circle->radius - sqrt(normal.lengthSquared());
//<< "\n";
c->normal = normal;
}
return collide;
}
int EXPORT_API ProcessSpheresCollisionsVsMovingMesh(IBroadphase* tree, btVector3* posA, btVector3* predPosA, float radius, int pointsCount, btVector3* posB, btVector3* predPosB, Triangle* triangles, bool twoSidedCollisions, float staticFric, float kineticFric)
{
int collisionTriIds[MAX_COLCOUNT];
int narrowPhaseColCount = 0;
btScalar radiusSq = radius * radius;
btScalar invMass = 1.0f;
//#pragma omp parallel for
for (int k = 0; k < pointsCount; k++)
{
btVector3 totalResponse(0, 0, 0);
int broadphaseColCount = tree->IntersectSphere(predPosA[k], radius, collisionTriIds);
for (int i = 0; i < broadphaseColCount; i++)
{
Triangle tri = triangles[collisionTriIds[i]];
const btVector3 pA = predPosB[tri.pointAid];
const btVector3 pB = predPosB[tri.pointBid];
const btVector3 pC = predPosB[tri.pointCid];
btVector3 bar;
Barycentric2(pA, pB, pC, predPosA[k], bar);
btVector3 contactPoint = pA * bar.x() + pB * bar.y() + pC * bar.z();
btVector3 triNormal;
calculateNormal(pA, pB, pC, triNormal);
btVector3 normal = predPosA[k] - contactPoint;
float distSq = normal.length2();
if (distSq > radiusSq)
continue;
float dist = btSqrt(distSq);
if (dist < 0.00001f)
continue;
normal /= dist;
if (!twoSidedCollisions && btDot(normal, triNormal) < 0)
{
normal = -normal;
continue;
}
btScalar C = radius - dist;
btVector3 responseVec = normal * C;
predPosA[k] += responseVec;
//FRICTION
btVector3 contactPointPred = predPosB[tri.pointAid] * bar.x() + predPosB[tri.pointBid] * bar.y() + predPosB[tri.pointCid] * bar.z();
btVector3 contactPointInitial = posB[tri.pointAid] * bar.x() + posB[tri.pointBid] * bar.y() + posB[tri.pointCid] * bar.z();
btVector3 dpf = (predPosA[k] - posA[k]) - (contactPointPred - contactPointInitial);
btVector3 dpt = dpf - btDot(dpf, normal) * normal;
float ldpt = dpt.length();
if (ldpt < 0.000001f)
continue;
if (ldpt < staticFric * C)
predPosA[k] -= dpt;
else
predPosA[k] -= dpt * btMin(kineticFric * C / ldpt, 1.0f);
totalResponse += responseVec;
narrowPhaseColCount++;
}
}
return narrowPhaseColCount;
}
int EXPORT_API ProjectSoftbodyCollisionConstraintsWithFriction(IBroadphase* tree, btVector3* posA, btVector3* predPosA, float radius, int pointsCount, float KsA, btVector3* posB, btVector3* predPosB, Triangle* triangles, float KsB, bool twoSidedCollisions, float staticFric, float kineticFric)
{
int collisionTriIds[MAX_COLCOUNT];
int narrowPhaseColCount = 0;
btScalar radiusSq = radius * radius;
btScalar invMass = 1.0f;
//#pragma omp parallel for
for (int k = 0; k < pointsCount; k++)
{
btVector3 totalResponse(0, 0, 0);
int broadphaseColCount = tree->IntersectSphere(predPosA[k], radius, collisionTriIds);
for (int i = 0; i < broadphaseColCount; i++)
{
Triangle tri = triangles[collisionTriIds[i]];
const btVector3 pA = predPosB[tri.pointAid];
const btVector3 pB = predPosB[tri.pointBid];
const btVector3 pC = predPosB[tri.pointCid];
btVector3 bar;
Barycentric2(pA, pB, pC, predPosA[k], bar);
btVector3 contactPoint = pA * bar.x() + pB * bar.y() + pC * bar.z();
btVector3 triNormal;
calculateNormal(pA, pB, pC, triNormal);
btVector3 normal = predPosA[k] - contactPoint;
float distSq = normal.length2();
if (distSq > radiusSq)
continue;
float dist = btSqrt(distSq);
if (dist < 0.00001f)
continue;
normal /= dist;
if (!twoSidedCollisions && btDot(normal, triNormal) < 0)
{
normal = -normal;
continue;
}
btScalar C = radius - dist;
btScalar s = invMass + invMass * bar.x() * bar.x() + invMass * bar.y() * bar.y() + invMass * bar.z() * bar.z();
if (s == 0.0f)
return narrowPhaseColCount;
btVector3 responseVec = normal * C / s;
predPosA[k] += responseVec * KsA;
predPosB[tri.pointAid] -= responseVec * bar.x() * KsB;
predPosB[tri.pointBid] -= responseVec * bar.y() * KsB;
predPosB[tri.pointCid] -= responseVec * bar.z() * KsB;
// Vector4 contactPointPred = contactPoint - responseVec;
btVector3 contactPointPred = predPosB[tri.pointAid] * bar.x() + predPosB[tri.pointBid] * bar.y() + predPosB[tri.pointCid] * bar.z();
btVector3 contactPointInitial = posB[tri.pointAid] * bar.x() + posB[tri.pointBid] * bar.y() + posB[tri.pointCid] * bar.z();
btVector3 nf = normal;
btVector3 dpf = (predPosA[k] - posA[k]) - (contactPointPred - contactPointInitial);
btVector3 dpt = dpf - btDot(dpf, nf) * nf;
// float d = radiusSum - distance;
float ldpt = dpt.length();
if (ldpt < 0.00001f)
continue;
if (ldpt < staticFric * C)
{
// Vector3 delta = dpt / invMassSum;
btVector3 delta = dpt / s;
predPosA[k] -= delta * invMass * KsA;
predPosB[tri.pointAid] += delta * bar.x() * invMass * KsB;
predPosB[tri.pointBid] += delta * bar.y() * invMass * KsB;
predPosB[tri.pointCid] += delta * bar.z() * invMass * KsB;
}
else
{
// Vector3 delta = dpt * Mathf.min(kineticFric * d / ldpt, 1.0f) / invMassSum;
btVector3 delta = dpt * btMin(kineticFric * C / ldpt, 1.0f) / s;
predPosA[k] -= delta * invMass * KsA;
predPosB[tri.pointAid] += delta * bar.x() * invMass * KsB;
predPosB[tri.pointBid] += delta * bar.y() * invMass * KsB;
predPosB[tri.pointCid] += delta * bar.z() * invMass * KsB;
}
totalResponse += responseVec;
narrowPhaseColCount++;
}
}
return narrowPhaseColCount;
}
int EXPORT_API ProjectSoftbodyCollisionConstraints(IBroadphase* tree, btVector3* predPosA, float radius, int pointsCount, float KsA, btVector3* predPosB, Triangle* triangles, float KsB, bool twoSidedCollisions)
{
int collisionTriIds[MAX_COLCOUNT];
int narrowPhaseColCount = 0;
btScalar radiusSq = radius * radius;
btScalar invMass = 1.0f;
//#pragma omp parallel for
for (int k = 0; k < pointsCount; k++)
{
btVector3 totalResponse(0, 0, 0);
int broadphaseColCount = tree->IntersectSphere(predPosA[k], radius, collisionTriIds);
for (int i = 0; i < broadphaseColCount; i++)
{
Triangle tri = triangles[collisionTriIds[i]];
const btVector3 pA = predPosB[tri.pointAid];
const btVector3 pB = predPosB[tri.pointBid];
const btVector3 pC = predPosB[tri.pointCid];
btVector3 bar;
Barycentric2(pA, pB, pC, predPosA[k], bar);
btVector3 contactPoint = pA * bar.x() + pB * bar.y() + pC * bar.z();
btVector3 triNormal;
calculateNormal(pA, pB, pC, triNormal);
btVector3 normal = predPosA[k] - contactPoint;
float distSq = normal.length2();
if (distSq > radiusSq)
continue;
float dist = btSqrt(distSq);
if (dist < 0.001f)
continue;
normal /= dist;
if (!twoSidedCollisions && btDot(normal, triNormal) < 0)
{
normal = -normal;
continue;
}
btScalar C = radius - dist;
btScalar s = invMass + invMass * bar.x() * bar.x() + invMass * bar.y() * bar.y() + invMass * bar.z() * bar.z();
if (s == 0.0f)
return narrowPhaseColCount;
btVector3 responseVec = normal * C / s;
predPosA[k] += responseVec * KsA;
predPosB[tri.pointAid] -= responseVec * bar.x() * KsB;
predPosB[tri.pointBid] -= responseVec * bar.y() * KsB;
predPosB[tri.pointCid] -= responseVec * bar.z() * KsB;
totalResponse += responseVec;
narrowPhaseColCount++;
}
//if (narrowPhaseColCount > 0)
//{
// predPosA[k] += totalResponse / (btScalar)narrowPhaseColCount;
//}
}
return narrowPhaseColCount;
}
Grid::Grid(int blockSize, Map* map)
: blockSize(blockSize)
{
int arraySize = blockSize * blockSize;
// Preallocate
indexedModel.positions.resize(arraySize);
indexedModel.normals.resize(arraySize);
indexedModel.texCoords.resize(arraySize);
for (int i = 0, var = 0; i < blockSize; i += 2)
{
for (int j = 0; j < blockSize; j += 2)
{
float height = 2;
float height1 = 0;
float height2 = 0;
float height3 = 0;
float height4 = 0;
if (map->getMapTerrainForCoordinate(glm::vec3(j, i, 0)) == BLOCKED)
height1 = height;
if (map->getMapTerrainForCoordinate(glm::vec3(j, i + 1, 0)) == BLOCKED)
height2 = height;
if (map->getMapTerrainForCoordinate(glm::vec3(j + 1, i, 0)) == BLOCKED)
height3 = height;
if (map->getMapTerrainForCoordinate(glm::vec3(j + 1, i + 1, 0)) == BLOCKED)
height4 = height;
if (map->getMapTerrainForCoordinate(glm::vec3(j, i, 0)) == WATER)
height1 = -height;
if (map->getMapTerrainForCoordinate(glm::vec3(j, i + 1, 0)) == WATER)
height2 = -height;
if (map->getMapTerrainForCoordinate(glm::vec3(j + 1, i, 0)) == WATER)
height3 = -height;
if (map->getMapTerrainForCoordinate(glm::vec3(j + 1, i + 1, 0)) == WATER)
height4 = -height;
glm::vec3 pos1 = glm::vec3(j, i, height1);
glm::vec3 pos2 = glm::vec3(j, i + 1, height2);
glm::vec3 pos3 = glm::vec3(j + 1, i, height3);
glm::vec3 pos4 = glm::vec3(j + 1, i + 1, height4);
// Positions
indexedModel.positions[var] = pos1;
indexedModel.positions[var + 1] = pos2;
indexedModel.positions[var + 2] = pos3;
indexedModel.positions[var + 3] = pos4;
// TexCoords
indexedModel.texCoords[var] = glm::vec2(pos1.x / blockSize, pos1.y / blockSize);
indexedModel.texCoords[var + 1] = glm::vec2(pos2.x / blockSize, pos2.y / blockSize);
indexedModel.texCoords[var + 2] = glm::vec2(pos3.x / blockSize, pos3.y / blockSize);
indexedModel.texCoords[var + 3] = glm::vec2(pos4.x / blockSize, pos4.y / blockSize);
var += 4;
}
}
// Calc Normals
int indexPos = 0;
for (int i = 0; i < blockSize; i += 2)
{
for (int j = 0; j < blockSize; j += 2)
{
indexedModel.normals[indexPos] = calculateNormal(indexedModel.positions[indexPos]);
indexedModel.normals[indexPos + 1] = calculateNormal(indexedModel.positions[indexPos + 1]);
indexedModel.normals[indexPos + 2] = calculateNormal(indexedModel.positions[indexPos + 2]);
indexedModel.normals[indexPos + 3] = calculateNormal(indexedModel.positions[indexPos + 3]);
indexPos += 4;
}
}
int halfblock = blockSize / 2.0f;
indexedModel.indices.resize((halfblock * (blockSize - 1) + (halfblock - 1) * (blockSize - 1)) * 6);
for (int i = 0, prevFirst = 0, newFirst = 0, deslocamento = 0, ultimo = 0, var = 0; i < blockSize - 1; i++)
{
for (int j = 0, aux = 0; j < blockSize - 1; j++)
{
indexedModel.indices[var + 0] = deslocamento + j + 3 + aux;
indexedModel.indices[var + 1] = deslocamento + j + 1 + aux;
indexedModel.indices[var + 2] = newFirst + j + aux;
indexedModel.indices[var + 3] = newFirst + j + aux;
indexedModel.indices[var + 4] = newFirst + j + 2 + aux;
indexedModel.indices[var + 5] = deslocamento + j + 3 + aux;
var += 6;
if (j == 0)
prevFirst = deslocamento + j + 1 + aux;
ultimo = deslocamento + j + 3 + aux;
aux = j + 1;
}
newFirst = (prevFirst + 1) - ((prevFirst + 1) - prevFirst);
if (i % 2 == 0)
deslocamento = ultimo;
else
deslocamento = ultimo - ((blockSize - 2) * 2 - 1) - 3;
}
}
void Face :: setVertexV4(Vertex* v4)
{
vertexVector[3] = v4;
normal = calculateNormal();
center = calculateCenter();
}
