I am making a 3D game using SFML. I want to use normals to check if the triangle need to be draw in a terrain (triangle) mesh.Here is my code:
vec3d line1, line2, normal;
line1.x = terrain.tris[i].p[0].x - terrain.tris[i].p[1].x;
line1.y = terrain.tris[i].p[0].y - terrain.tris[i].p[1].y;
line1.z = terrain.tris[i].p[0].z - terrain.tris[i].p[1].z;
line2.x = terrain.tris[i].p[1].x - terrain.tris[i].p[2].x;
line2.y = terrain.tris[i].p[1].y - terrain.tris[i].p[2].y;
line2.z = terrain.tris[i].p[1].z - terrain.tris[i].p[2].z;
normal.x = line1.y * line2.z - line1.z * line2.y;
normal.y = line1.z * line2.x - line1.x * line2.z;
normal.z = line1.x * line2.y - line1.y * line2.x;
vec3d vCameraRay = Vector_Sub(terrain.tris[i].p[0], cam.pos);
if (Vector_DotProduct(normal, vCameraRay) < 0.0f){
do_something();
}
Vector_DotProduct:
float Vector_DotProduct(vec3d& v1, vec3d& v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
Vector_sub:
vec3d Vector_Sub(vec3d& v1, vec3d& v2) {
return { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
}
And the vec3d is just a struct that contains
float x, y, z;
But whenever I run the program, I always get this
problem
The triangles that should be displayed was considered "Not visable" by my program(The normal of it is wrong), but the calculation seems right to me!
The code for producing triangle grid:
for (int i = 0; i < 2; i++) {
for (int y = 0; y < wds; y++) {
for (int x = 0; x < wds; x++) {
if (x + 1 < wds && y + 1 < wds) {
vec3d point[3];
switch (i) {
case 0:
point[0] = { (float)(y + 1) * scl + p.x, height * h[y + 1][x + 1], (float)(x + 1) * scl + p.z };
point[1] = { (float)y * scl + p.x, height * h[y][x], (float)x * scl + p.z };
point[2] = { (float)y * scl + p.x, height * h[y][x + 1], (float)(x + 1) * scl + p.z };
break;
case 1:
point[0] = { (float)(y + 1) * scl + p.x, height * h[y + 1][x + 1], (float)(x + 1) * scl + p.z };
point[2] = { (float)y * scl + p.x, height * h[y][x], (float)x * scl + p.z };
point[1] = { (float)(y + 1) * scl + p.x, height * h[y + 1][x], (float)x * scl + p.z };
break;
};
triangle out = { point[0], point[1], point[2] };
tris.push_back(out);
}
}
}
}
The wds is for the size of the grid(side length), the scl is the size of per grid, the h is the height map(two dimentions), and p is the position of the upper left corner.
My 3D point to camera point code:
float mx = p.x - pos.x;
float my = p.y - pos.y;
float mz = p.z - pos.z;
float dx = cos(rot.y) * (sin(rot.z) * my + cos(rot.z) * mx) - sin(rot.y) * mz;
float dy = sin(rot.x) * (cos(rot.y) * mz + sin(rot.y) * (sin(rot.z) * my + cos(rot.z) * mx)) + cos(rot.x) * (cos(rot.z) * my + sin(rot.z) * mx);
float dz = cos(rot.x) * (cos(rot.y) * mz + sin(rot.y) * (sin(rot.z) * my + cos(rot.z) * mx)) - sin(rot.x) * (cos(rot.z) * my + sin(rot.z) * mx);
return { dx, dy, dz };
The rot is the rotation of the camera, p is the position of the camera, and the pos is the 3D point I want to transfer to camera point.
I have been working on this problem for almost a week, but nothing seems to work.It will be a lot of help if you guys can find the problem. Thanks in advance!
Full Code
Init.h:
#ifndef _INIT_H_
#define _INIT_H_
#define WIDTH 1200
#define HEIGHT 800
#endif
Noise.h: noice function
#pragma once
#ifndef _NOISE_H_
#define _NOISE_H_
extern int primeIndex;
extern int numOctaves;
extern double persistence;
extern int primes[10][3];
#endif
#include <math.h>
float Noise(int i, int x, int y);
float SmoothedNoise(int i, int x, int y);
float Interpolate(float a, float b, float x);
float InterpolatedNoise(int i, float x, float y);
float noise(float x, float y);
Noise.cpp:
#include "Noise.h"
int primeIndex = 0;
int numOctaves = 7;
double persistence = 0.5;
int primes[10][3] = {
{ 995615039, 600173719, 701464987 },
{ 831731269, 162318869, 136250887 },
{ 174329291, 946737083, 245679977 },
{ 362489573, 795918041, 350777237 },
{ 457025711, 880830799, 909678923 },
{ 787070341, 177340217, 593320781 },
{ 405493717, 291031019, 391950901 },
{ 458904767, 676625681, 424452397 },
{ 531736441, 939683957, 810651871 },
{ 997169939, 842027887, 423882827 }
};
float Noise(int i, int x, int y) {
int n = x + y * 57;
n = (n << 13) ^ n;
int a = primes[i][0], b = primes[i][1], c = primes[i][2];
int t = (n * (n * n * a + b) + c) & 0x7fffffff;
return 1.0 - (float)(t) / 1073741824.0;
}
float SmoothedNoise(int i, int x, int y) {
float corners = (Noise(i, x - 1, y - 1) + Noise(i, x + 1, y - 1) +
Noise(i, x - 1, y + 1) + Noise(i, x + 1, y + 1)) / 16,
sides = (Noise(i, x - 1, y) + Noise(i, x + 1, y) + Noise(i, x, y - 1) +
Noise(i, x, y + 1)) / 8,
center = Noise(i, x, y) / 4;
return corners + sides + center;
}
float Interpolate(float a, float b, float x) {
float ft = x * 3.1415927,
f = (1 - cos(ft)) * 0.5;
return a * (1 - f) + b * f;
}
float InterpolatedNoise(int i, float x, float y) {
int integer_X = x;
float fractional_X = x - integer_X;
int integer_Y = y;
float fractional_Y = y - integer_Y;
float v1 = SmoothedNoise(i, integer_X, integer_Y),
v2 = SmoothedNoise(i, integer_X + 1, integer_Y),
v3 = SmoothedNoise(i, integer_X, integer_Y + 1),
v4 = SmoothedNoise(i, integer_X + 1, integer_Y + 1),
i1 = Interpolate(v1, v2, fractional_X),
i2 = Interpolate(v3, v4, fractional_X);
return Interpolate(i1, i2, fractional_Y);
}
float noise(float x, float y) {
float total = 0,
frequency = pow(2, numOctaves),
amplitude = 1;
for (int i = 0; i < numOctaves; ++i) {
frequency /= 2;
amplitude *= persistence;
total += InterpolatedNoise((primeIndex + i) % 10,
x / frequency, y / frequency) * amplitude;
}
return total / frequency;
}
Struct.h:
#pragma once
#ifndef _STRUCT_H_
#define _STRUCT_H_
#endif
#include <vector>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <list>
#include "Init.h"
struct vec3d {
float x = 0;
float y = 0;
float z = 0;
};
struct vec2d {
float x = 0;
float y = 0;
};
struct triangle {
vec3d p[3];
int color[3] = { 255, 255, 255 };
vec3d normal;
};
Terrain.h: terrain generation
#pragma once
#ifndef _TERRAIN_H_
#define _TERRAIN_H_
#endif
#include <vector>
#include "Struct.h"
#include "Noise.h"
#define wds 50
#define scl 20
#define width 1000
#define height 120
struct Terrain {
public:
std::vector<triangle> tris;
vec3d p = { -width / 2, 0.0f, -width / 2 };
float h[wds][wds];
void triangle_Strip();
};
Terrain.cpp:
#include "Terrain.h"
void Terrain::make_value() {
for (int y = 0; y < wds; y++) {
for (int x = 0; x < wds; x++) {
int a = abs(p.z / scl + x), b = abs(p.x / scl + y);
h[y][x] = noise(a, b) * 30;
}
}
}
void Terrain::triangle_Strip() {
tris.clear();
for (int i = 0; i < 2; i++) {
for (int y = 0; y < wds; y++) {
for (int x = 0; x < wds; x++) {
if (x + 1 < wds && y + 1 < wds) {
vec3d point[3];
switch (i) {
case 0:
point[0] = { (float)(y + 1) * scl + p.x, height * h[y + 1][x + 1], (float)(x + 1) * scl + p.z };
point[1] = { (float)y * scl + p.x, height * h[y][x], (float)x * scl + p.z };
point[2] = { (float)y * scl + p.x, height * h[y][x + 1], (float)(x + 1) * scl + p.z };
break;
case 1:
point[0] = { (float)(y + 1) * scl + p.x, height * h[y + 1][x + 1], (float)(x + 1) * scl + p.z };
point[2] = { (float)y * scl + p.x, height * h[y][x], (float)x * scl + p.z };
point[1] = { (float)(y + 1) * scl + p.x, height * h[y + 1][x], (float)x * scl + p.z };
break;
};
triangle out = { point[0], point[1], point[2] };
tris.push_back(out);
}
}
}
}
}
Camera.h: camera class, get3dcoord which is get camera point, get2dcoord which is get screen point
#pragma once
#ifndef _CAMERA_H_
#define _CAMERA_H_
#endif
#include "Mat.h"
#include "Init.h"
#include "Struct.h"
class Cam {
public:
vec3d pos;
vec3d rot;
float fov;
float speed;
Cam(vec3d p, vec3d r, float f, float s);
vec3d get3dcoord(vec3d p);
vec3d get2dcoord(vec3d p);
};
Camera.cpp:
#include "Camera.h"
Cam::Cam(vec3d p, vec3d r, float f, float s) {
pos = p;
rot = r;
fov = f;
speed = s;
}
vec3d Cam::get3dcoord(vec3d p) {
float mx = p.x - pos.x;
float my = p.y - pos.y;
float mz = p.z - pos.z;
float dx = cos(rot.y) * (sin(rot.z) * my + cos(rot.z) * mx) - sin(rot.y) * mz;
float dy = sin(rot.x) * (cos(rot.y) * mz + sin(rot.y) * (sin(rot.z) * my + cos(rot.z) * mx)) + cos(rot.x) * (cos(rot.z) * my + sin(rot.z) * mx);
float dz = cos(rot.x) * (cos(rot.y) * mz + sin(rot.y) * (sin(rot.z) * my + cos(rot.z) * mx)) - sin(rot.x) * (cos(rot.z) * my + sin(rot.z) * mx);
return { dx, dy, dz };
}
vec3d Cam::get2dcoord(vec3d p) {
float e = (float)tan(fov / 2) * (float)(WIDTH / 2);
float x = (WIDTH / 2) + (e * p.x) / p.z;
float y = (HEIGHT / 2) + (e * p.y) / p.z;
return { x, y, 0 };
}
3D engine.h: main
#pragma once
#ifndef _3D_ENGINE_H_
#define _3D_ENGINE_H_
#endif
#include <SFML/Graphics.hpp>
#include <SFML/System.hpp>
#include <SFML/Window.hpp>
#include <iostream>
#include <stdlib.h>
#include <sstream>
#include <list>
#include "Struct.h"
#include "Camera.h"
#include "Init.h"
#include "Noise.h"
#include "Terrain.h"
#define endl "\n"
void draw_triangle(vec3d p1, vec3d p2, vec3d p3, int color[]);
vec3d Vector_Sub(vec3d& v1, vec3d& v2);
float Vector_DotProduct(vec3d& v1, vec3d& v2);
3D engine.cpp:
#include "3D engine.h"
sf::RenderWindow window(sf::VideoMode(WIDTH, HEIGHT), "3D game in progress");
const sf::Vector2i windowCenter(WIDTH / 2, HEIGHT / 2);
Cam cam({ 0.0f, -40.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, 90, 2.0f);
Terrain terrain;
sf::VertexArray TriangleToDraw(sf::Triangles);
void draw_triangle(vec3d p1, vec3d p2, vec3d p3, int color[]) {
sf::VertexArray tri(sf::Triangles, 3);
tri[0].position = sf::Vector2f(p1.x, p1.y);
tri[1].position = sf::Vector2f(p2.x, p2.y);
tri[2].position = sf::Vector2f(p3.x, p3.y);
tri[0].color = sf::Color((int)color[0], (int)color[1], (int)color[2]);
tri[1].color = sf::Color((int)color[0], (int)color[1], (int)color[2]);
tri[2].color = sf::Color((int)color[0], (int)color[1], (int)color[2]);
TriangleToDraw.append(tri[0]);
TriangleToDraw.append(tri[1]);
TriangleToDraw.append(tri[2]);
}
vec3d Vector_Sub(vec3d& v1, vec3d& v2) {
return { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
}
float Vector_DotProduct(vec3d& v1, vec3d& v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
int main() {
window.setMouseCursorVisible(false);
sf::Mouse::setPosition(windowCenter, window);
terrain.make_value();
terrain.triangle_Strip();
while (window.isOpen()) {
TriangleToDraw.clear();
sf::Event event;
while (window.pollEvent(event)) {
if (event.type == sf::Event::Closed) {
window.close();
}
if ((event.type == sf::Event::MouseLeft || event.type == sf::Event::MouseMoved) && sf::Mouse::getPosition(window) != windowCenter) {
sf::Vector2i pos = sf::Mouse::getPosition(window);
int x_a = pos.x;
int y_a = pos.y;
float movex = (float)(x_a - windowCenter.x) / 500.0f;
float movey = (float)(y_a - windowCenter.y) / 500.0f;
cam.rot.x -= movey;
cam.rot.y += movex;
sf::Mouse::setPosition(windowCenter, window);
}
}
float x = sin(cam.rot.y) * cam.speed; float z = cos(cam.rot.y) * cam.speed;
if (sf::Keyboard::isKeyPressed(sf::Keyboard::W)) { cam.pos.x -= x; cam.pos.z -= z; /*terrain.p.x -= x; terrain.p.z -= z;*/ }
if (sf::Keyboard::isKeyPressed(sf::Keyboard::S)) { cam.pos.x += x; cam.pos.z += z; /*terrain.p.x += x; terrain.p.z += z;*/ }
if (sf::Keyboard::isKeyPressed(sf::Keyboard::A)) { cam.pos.x += z; cam.pos.z -= x; /*terrain.p.x += z; terrain.p.z -= x;*/ }
if (sf::Keyboard::isKeyPressed(sf::Keyboard::D)) { cam.pos.x -= z; cam.pos.z += x; /*terrain.p.x -= z; terrain.p.z += x;*/ }
if (sf::Keyboard::isKeyPressed(sf::Keyboard::Space)) cam.pos.y += cam.speed;
if (sf::Keyboard::isKeyPressed(sf::Keyboard::LSHIFT)) cam.pos.y -= cam.speed;
window.clear(sf::Color(0, 0, 0));
std::vector<triangle> triangles;
for (int i = 0, len = terrain.tris.size(); i < len; i++) {
std::vector<vec3d> projected(3);
for (int r = 0; r < 3; r++) projected[r] = cam.get3dcoord(terrain.tris[i].p[r]);
vec3d line1, line2, normal;
line1.x = projected[0].x - projected[1].x;
line1.y = projected[0].y - projected[1].y;
line1.z = projected[0].z - projected[1].z;
line2.x = projected[1].x - projected[2].x;
line2.y = projected[1].y - projected[2].y;
line2.z = projected[1].z - projected[2].z;
normal.x = line1.y * line2.z - line1.z * line2.y;
normal.y = line1.z * line2.x - line1.x * line2.z;
normal.z = line1.x * line2.y - line1.y * line2.x;
float l = sqrtf(normal.x * normal.x + normal.y * normal.y + normal.z * normal.z);
normal.x /= l; normal.y /= l; normal.z /= l;
vec3d vCameraRay1 = Vector_Sub(projected[0], cam.pos);
if (Vector_DotProduct(normal, vCameraRay1) < 0.0f && projected[0].z < 0.0f && projected[1].z < 0.0f && projected[2].z < 0.0f/*avoid points behind the camera to be projected*/) {
vec3d light = { 0.0f, 0.0f, 1.0f };
float lNormal = sqrtf(powf(light.x, 2) + powf(light.y, 2) + powf(light.z, 2));
light.x /= lNormal; light.y /= lNormal; light.z /= lNormal;
float dp = std::max(0.3f, Vector_DotProduct(light, normal));
int c = 255 * dp;
triangles.push_back({projected[0], projected[1], projected[2], {c, c, c}});
}
}
std::sort(triangles.begin(), triangles.end(), [](triangle& t1, triangle& t2)
{
float z1 = (t1.p[0].z + t1.p[1].z + t1.p[2].z) / 3.0f;
float z2 = (t2.p[0].z + t2.p[1].z + t2.p[2].z) / 3.0f;
return z1 < z2;
});
for (triangle tri : triangles) {
draw_triangle(cam.get2dcoord(tri.p[0]), cam.get2dcoord(tri.p[1]), cam.get2dcoord(tri.p[2]), tri.color);
}
window.draw(TriangleToDraw);
window.display();
}
return 0;
}
One of the triangle that had the wrong normal:
Normal: -0.08
vCameraRay: -588.2, 19.0, -662.5
Vector Dotproduct: -74.7
Triangle Point1: 19.03, -35.10, -75.69
Triangle Point2: -1.28, -27.57, -92.94
Triangle Point3: -0.96, -25.79, -71.35
Camera position: 2.20, 627.26, 0.03
One of the triangle that had the wrong Dot Product:
Normal: 0.59
vCameraRay: 468.41, 13.59, -634.75
Vector DotProduct: -55.05
Triangle Point1: 13.59, -7.29, -55.05
Triangle Point2: 19.19, 7.04, -37.72
Trianlge Point3: 0.00, 9.75, -28.36
Camera pos: 0.00, 627.45, 0.00
I have 2 function to either calculate a point on a spline, quadratic or cubic:
struct vec2 {float x, y;};
vec2 spline_quadratic(vec2 & a, vec2 & b, vec2 & c, float t) {
return {
(1 - t) * (1 - t) * p1.x + 2 * (1 - t) * t * p2.x + t * t * p3.x,
(1 - t) * (1 - t) * p1.y + 2 * (1 - t) * t * p2.y + t * t * p3.y
};
}
vec2 spline_cubic(vec2 & a, vec2 & b, vec2 & c, vec2 & d, float t){
return {
//B(t) = (1-t)**3 p0 + 3(1 - t)**2 t P1 + 3(1-t)t**2 P2 + t**3 P3
(1 - t) * (1 - t) * (1 - t) * p1.x + 3 * (1 - t) * (1 - t) * t * p2.x + 3 * (1 - t) * t * t * p3.x + t * t * t * p4.x,
(1 - t) * (1 - t) * (1 - t) * p1.y + 3 * (1 - t) * (1 - t) * t * p2.y + 3 * (1 - t) * t * t * p3.y + t * t * t * p4.y
};
Is it possible to join several curves of an array of points?
I'm looking to make a function that has this signature:
vector<vec2> spline_join(vector<vec2> & points, int segments = 16){
vector<vec2> spline_points;
for(int i = 0; i < points.size()-2; ++i){
for(int div = 0; div < segments; ++div){
spline_points.push_back(spline_quadratic(points[0], points[1], points[2], 1.f/segments);
}
}
}
I've read that it requires interpolation, but I'm not sure... What would the code look like? I've searched and I can't find relevant question and answers...
I've seen there are libraries, but I'm looking for a shorter implementation.
Edit: I've tried the question and answer here and apparently this is what I want:
Joining B-Spline segments in OpenGL / C++
The code is not really clean but after some cleaning, it does work.
I've cleaned this answer Joining B-Spline segments in OpenGL / C++
This is not an Hermite spline, an hermite spline passes through the points, a B-spline does not.
Here is what worked and the result
float B0(float u) {
//return float(pow(u - 1, 3) / 6.0);
// (1-t)*(1-t)*(1-t)/6.f
return float(pow(1-u, 3) / 6.0);
}
float B1(float u) {
return float((3 * pow(u, 3) - 6 * pow(u, 2) + 4) / 6.0);
// (3 * t * t * t - 6 * t * t + 4) / 6
}
float B2(float u) {
return float((-3 * pow(u, 3) + 3 * pow(u, 2) + 3 * u + 1) / 6.0);
// (-3 * t * t * t + 3 * t * t + 3 * t + 1) / 6
}
float B3(float u) {
return float(pow(u, 3) / 6.0);
// t * t * t / 6
}
vector<Vec2> computeBSpline(vector<Vec2>& points) {
vector<Vec2> result;
int MAX_STEPS = 100;
int NUM_OF_POINTS = points.size();
for (int i = 0; i < NUM_OF_POINTS - 3; i++)
{
//cout << "Computing for P" << i << " P " << i + 1 << " P " << i + 2 << " P " << i + 3 << endl;
for (int j = 0; j <= MAX_STEPS; j++)
{
float u = float(j) / float(MAX_STEPS);
float Qx =
B0(u) * points[i].x
+ B1(u) * points[i + 1].x
+ B2(u) * points[i + 2].x
+ B3(u) * points[i + 3].x;
float Qy =
B0(u) * points[i].y
+ B1(u) * points[i + 1].y
+ B2(u) * points[i + 2].y
+ B3(u) * points[i + 3].y;
result.push_back({ Qx, Qy });
//cout << count << '(' << Qx << ", " << Qy << ")\n";
}
}
return result;
}
I am programming a physics simulation with few particles (typically 3, no more than 5).
In a condensed version my code structure like this:
#include<iostream>
class Particle{
double x; // coordinate
double m; // mass
};
void performStep(Particle &p, double &F_external){
p.x += -0.2*p.x + F_external/p.m; // boiled down, in reality complex calculation, not important here
}
int main(){
dt = 0.001; // time step, not important
Particle p1;
p1.x = 5; // some random number for initialization, in reality more complex but not important here
p.m = 1;
Particle p2;
p2.x = -1; // some random numbersfor initialization, in reality more complex but not important here
p.m = 2;
Particle p3;
p3.x = 0; // some random number for initialization, in reality more complex but not important here
p.m = 3;
double F_external = 0; // external forces
for(unsigned long long int i=0; i < 10000000000; ++i){ // many steps, typically 10e9
F_external = sin(i*dt);
performStep(p1, F_external);
performStep(p2, F_external);
performStep(p3, F_external);
}
std::cout << "p1.x: " << p1.x << std::endl;
std::cout << "p2.x: " << p2.x << std::endl;
std::cout << "p3.x: " << p3.x << std::endl;
}
I have determined with clock() that the performStep(p, F_external) call is the bottleneck in my code).
When I tried to do inline calculation, i.e. replace performStep(p1, F_external) by p1.x += -0.2*p1.x + F_external/p1.m; the calculation suddenly was roughly a factor of 2 faster. Note that performStep() in reality is about ~60 basic arithmetic calculations over ~20 lines, so the code becomes really bloated if I just inline it for every particle.
Why is that the case? I am compiling with MinGW64/g++ and the -O2 flag. I thought the compiler would optimize such things?
Edit:
Here is the function that is called. Note that in reality, I calculate all three coordinates x,y,z with a couple of different external forces. Variables which are not passed via the function are a member of SimulationRun. The algorithm is a fourth-order leapfrog algorithm.
void SimulationRun::performLeapfrog_z(const unsigned long long int& i, const double& x, const double& y, double& z, const double& vx, const double& vy, double& vz, const double& qC2U0,
const double& U0, const double& m, const double& C4, const double& B2, const double& f_minus, const double& f_z, const double& f_plus, const bool& bool_calculate_xy,
const double& Find, const double& Fheating) {
// probing for C4 == 0 and B2 == 0 saves some computation time
if (C4 == 0) {
Fz_C4_Be = 0;
}
if (B2 == 0 || !bool_calculate_xy) {
Fz_B2_Be = 0;
}
z1 = z + c1 * vz * dt;
if (C4 != 0 && !bool_calculate_xy) {
Fz_C4_Be = (-4) * q * C4 * U0 * z1 * z1 * z1;
}
else if (C4 != 0 && bool_calculate_xy) {
Fz_C4_Be = q * C4 * U0 * (-4 * z1 * z1 * z1 + 6 * z1 * (x * x + y * y));
}
if (B2 != 0 && bool_calculate_xy) {
Fz_B2_Be = q * B2 * (-vx * z1 * y + vy * z1 * x);
}
acc_z1 = (qC2U0 * (-2) * z1 + Find + Fz_C4_Be + Fz_B2_Be + Fheating) / m;
vz1 = vz + d1 * acc_z1 * dt;
z2 = z1 + c2 * vz1 * dt;
if (C4 != 0 && !bool_calculate_xy) {
Fz_C4_Be = (-4) * q * C4 * U0 * z2 * z2 * z2;
}
else if (C4 != 0 && bool_calculate_xy) {
Fz_C4_Be = q * C4 * U0 * (-4 * z2 * z2 * z2 + 6 * z2 * (x * x + y * y));
}
if (B2 != 0 && bool_calculate_xy) {
Fz_B2_Be = q * B2 * (-vx * z2 * y + vy * z2 * x);
}
acc_z2 = (qC2U0 * (-2) * z2 + +Find + Fz_C4_Be + Fz_B2_Be + Fheating) / m;
vz2 = vz1 + d2 * acc_z2 * dt;
z3 = z2 + c3 * vz2 * dt;
if (C4 != 0 && !bool_calculate_xy) {
Fz_C4_Be = (-4) * q * C4 * U0 * z3 * z3 * z3;
}
else if (C4 != 0 && bool_calculate_xy) {
Fz_C4_Be = q * C4 * U0 * (-4 * z3 * z3 * z3 + 6 * z3 * (x * x + y * y));
}
if (B2 != 0 && bool_calculate_xy) {
Fz_B2_Be = q * B2 * (-vx * z3 * y + vy * z3 * x);
}
acc_z3 = (qC2U0 * (-2) * z3 + Find + Fz_C4_Be + Fz_B2_Be + Fheating) / m;
vz3 = vz2 + d3 * acc_z3 * dt;
z = z3 + c4 * vz3 * dt;
vz = vz3;
}
Optimization is hard, even for compilers. Here are some optimization tips:
Since your performStep is hotspot, put it into a header file(in case that you split declaration and definition into header/source), then add inline keyword, like:
// at file xxx.h
inline void performStep(Particle &p, double F_external){
p.x += -0.2*p.x + F_external/p.m; // boiled down, in reality complex calculation, not important here
}
Upgrade your compiler, maybe to the latest.
use https://godbolt.org/ to check the assembly code. In this case, unnecessary dereference is the headache of performance.
I want to overload the * operator for my QUATERNION class but somehow I'm unable to use it at the same time with QUATERNIONs methods (Q4 example).
Is it possible to use somehow Q1*Q2.conjugated() without creating any temp variables?
C:\Users\xXx\Documents\Arduino\libraries\QUATERNION\extras\Quaternions\main.cpp|19|error:
cannot bind non-const lvalue reference of type 'QUATERNION&' to an
rvalue of type 'QUATERNION'|
QUATERNION Q1(1,2,3,4);
QUATERNION Q2(5,6,7,8);
QUATERNION Q2c = Q2.conjugated();
QUATERNION Q3 = Q1*Q2c; // work fine
QUATERNION Q4 = Q1*Q2.conjugated(); // not working
QUATERNION.h
#ifndef QUATERNION_H
#define QUATERNION_H
class QUATERNION
{
public:
float w;
float x;
float y;
float z;
QUATERNION(void);
QUATERNION(float,float,float,float);
float magnitude(void);
void normalize(void);
void scale(float);
QUATERNION normalized(void);
QUATERNION conjugated(void);
};
// operatory ==========================================
QUATERNION operator * (QUATERNION&,QUATERNION&);
QUATERNION operator * (QUATERNION&,float);
bool operator == (QUATERNION&,QUATERNION&);
// funkcje ============================================
QUATERNION Q_mul(QUATERNION&,QUATERNION&);
QUATERNION Q_scaled(QUATERNION&,float);
QUATERNION Q_fromAngular(const float*);
void Q_toAngular(QUATERNION&,float*,bool);
#endif // QUATERNION_H
QUATERNION.cpp
#include "QUATERNION.h"
#include "math.h"
QUATERNION::QUATERNION() : QUATERNION(1,0,0,0)
{
}
QUATERNION::QUATERNION(float w,float x,float y,float z){
this->w = w;
this->x = x;
this->y = y;
this->z = z;
}
float QUATERNION::magnitude(){
return sqrt(w*w+x*x+y*y+z*z);
}
void QUATERNION::normalize(){
float invMag = 1.0/magnitude();
scale(invMag);
}
void QUATERNION::scale(float scalar){
w *= scalar;
x *= scalar;
y *= scalar;
z *= scalar;
}
QUATERNION QUATERNION::normalized(){
QUATERNION q = QUATERNION(w,x,y,z);
q.normalize();
return q;
}
QUATERNION QUATERNION::conjugated(){
return QUATERNION(w,-x,-y,-z);
}
// operatory ==========================================
QUATERNION operator * (QUATERNION &A,QUATERNION &B){
return Q_mul(A,B);
}
QUATERNION operator * (QUATERNION &q,float scalar){
return Q_scaled(q,scalar);
}
bool operator == (QUATERNION &A,QUATERNION &B){
float epsilon = 1.0e-5;
return fabs(A.w-B.w)<=epsilon && fabs(A.x-B.x)<=epsilon
&& fabs(A.y-B.y)<=epsilon && fabs(A.z-B.z)<=epsilon;
}
// funkcje ============================================
QUATERNION Q_mul(QUATERNION &A,QUATERNION &B){
return QUATERNION(
A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, // w
A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, // x
A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, // y
A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w); // z
}
QUATERNION Q_scaled(QUATERNION &q,float scalar){
return QUATERNION(q.w*scalar,q.x*scalar,q.y*scalar,q.z*scalar);
}
QUATERNION Q_fromAngular(const float *w) {
float theta,q0,q1,q2,q3;
float dt = 1;
float x = w[0]*dt;
float y = w[1]*dt;
float z = w[2]*dt;
theta = sqrt(x*x + y*y + z*z);
if (theta<=1.0e-6) return QUATERNION(1,0,0,0);
q0 = cos(theta/2.0f);
q1 = sin(theta/2.0f)/theta * x;
q2 = sin(theta/2.0f)/theta * y;
q3 = sin(theta/2.0f)/theta * z;
return QUATERNION(q0,q1,q2,q3);
// w/theta - normalizacja wektora
}
void Q_toAngular(QUATERNION &q,float *angular,bool deg) {
// http://www.euclideanspace.com/physics/kinematics/angularvelocity/index.htm
float w=q.w,x=q.x,y=q.y,z=q.z;
if (w<0){
// w*=-1.0;
// x*=-1.0;
// y*=-1.0;
// z*=-1.0;
}
if (fabs(w)==1){
// unika dzielenia przez 0
// taki kwaternion nie zawiera rotacji
angular[0] = 0;
angular[1] = 0;
angular[2] = 0;
return;
}
// https://math.stackexchange.com/questions/3753611/quaternion-to-rotation-vector-sintheta-2-sqrt1-quaternion-w2
// theta = acos(w)*2
// sqrt(1-w*w) = sin(theta/2)
// float m = ( acos(w)*2.0 )/sqrt(1-w*w); // theta/sin(theta/2)
float theta = 2*acos(w);
float m = theta/sin(theta/2);
if (deg)
m*= 180.0/M_PI;
angular[0] = x*m;
angular[1] = y*m;
angular[2] = z*m;
}
In QUATERNION operator * (QUATERNION& A,QUATERNION& B);, you accept A and B by non-const reference. The compiler expects you to change them. That's obviously wrong for a multiplication. Add the missing const to both arguments, and to all other references in your code.
Also it appears that Angular might be a type. You currently use a float[3] for that.
I'm trying to code my custom implementation of Opengl glRotatef(angle,x,y,z) function.
I wrote the rotation matrix, but when I try to use it, the effect is not the same as the original function. Here is my code;
void mglRotate(float angle, float x, float y, float z)
{
float angle_rad = angle * (PI/180.0f);
float c = cos(angle_rad);
float s = sin(angle_rad);
float t = 1 - c;
float m[16] = {
c+x*x*t,y*x*t+z*s,z*x*t-y*s,0,
x*y*t-z*s,c+y*y*t,z*y*t+x*s,0,
x*z*t+y*s,y*z*t-x*s,z*z*t+c,0,
0,0,0,1
};
glMultMatrixf(m);
}
Where is my mistake?
There is a library glm, that does exactly the same thing as old openGL functions. You can compare your implementation with implementation in glm and figure it out :)
template <typename T>
GLM_FUNC_QUALIFIER detail::tmat4x4<T> rotate
(
detail::tmat4x4<T> const & m,
T const & angle,
detail::tvec3<T> const & v
)
{
T a = radians(angle);
T c = cos(a);
T s = sin(a);
detail::tvec3<T> axis = normalize(v);
detail::tvec3<T> temp = (T(1) - c) * axis;
detail::tmat4x4<T> Rotate(detail::tmat4x4<T>::null);
Rotate[0][0] = c + temp[0] * axis[0];
Rotate[0][1] = 0 + temp[0] * axis[1] + s * axis[2];
Rotate[0][2] = 0 + temp[0] * axis[2] - s * axis[1];
Rotate[1][0] = 0 + temp[1] * axis[0] - s * axis[2];
Rotate[1][1] = c + temp[1] * axis[1];
Rotate[1][2] = 0 + temp[1] * axis[2] + s * axis[0];
Rotate[2][0] = 0 + temp[2] * axis[0] + s * axis[1];
Rotate[2][1] = 0 + temp[2] * axis[1] - s * axis[0];
Rotate[2][2] = c + temp[2] * axis[2];
detail::tmat4x4<T> Result(detail::tmat4x4<T>::null);
Result[0] = m[0] * Rotate[0][0] + m[1] * Rotate[0][1] + m[2] * Rotate[0][2];
Result[1] = m[0] * Rotate[1][0] + m[1] * Rotate[1][1] + m[2] * Rotate[1][2];
Result[2] = m[0] * Rotate[2][0] + m[1] * Rotate[2][1] + m[2] * Rotate[2][2];
Result[3] = m[3];
return Result;
}
The one thing that seems wrong to me in your code is that you don't normalize the axis.