After getting accelerometer/gyroscope from MPU6050, I use this function:
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az,float mx, float my,float mz,double * quad) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
double* tempQuat = quad;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2)
;
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
*tempQuat++ = q0;
*tempQuat++ = q1;
*tempQuat++ = q2;
*tempQuat++ = q3;
}
To get quaternion value q0 ,q1 , q3. Then I use this function:
void MPU6050_getYawPitchRoll(double * ypr,double *qu)
{
double gx, gy, gz;
double *tempQ = qu;
double q0,q1,q2,q3;
float sqw = q0*q0;
float sqx = q1*q1;
float sqy = q2*q2;
float sqz = q3*q3;
q0= *tempQ++;
q1= *tempQ++;
q2= *tempQ++;
q3= *tempQ++;
gx = 2 * (q1 * q3 - q0 * q2);
gy = 2 * (q0 * q1 + q2 * q3);
gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
yaw = atan2(2* (q1 * q2 - q0 * q3), 2 * (q0 * q0 + q1 * q1) - 1) * 180 / 3.1416; // YAW
pitch = atan(gx / sqrt(gy * gy + gz * gz)) * 180 / 3.1416; // PITCH
roll = atan(gy / sqrt(gx * gx + gz * gz)) * 180 / 3.1416; // ROLL
psi = atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0*q0 + 2 * q1* q1 - 1)* 180 / 3.1416; // psi
theta = -asin(2 * q1 * q3 + 2 * q0 * q2)* 180 / 3.1416; // theta
phi = atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1)* 180 / 3.1416; // phi
}
To get yaw, pitch, roll, psi, theta and phi.
In PC, I use openGL to draw 3D object. How can I use yaw, pitch, roll, psi, theta and phi to control that 3D object (rotation, translation)? Is that right?
glRotatef(-Yaw, 0.0f, 1.0f, 0.0f);
glRotatef(-Pitch, 0.0f, 0.0f, 1.0f);
glRotatef(-Roll, 1.0f, 0.0f, 0.0f);
you doing it all the old way, why don't you use GLM ?
I did it this way in my project with cinder
I read _quat from MPU and arduino ...
vec4 axis = toAxisAngle(_quat);
// ...
rotate(axis[0], -axis[1], axis[3], axis[2]); // Note the orders and signs
and toAxisAngle :
/**
* Converts the quaternion into a float array consisting of: rotation angle
* in radians, rotation axis x,y,z
*
* #return 4-element float array
*/
static const float EPS = 1.1920928955078125E-7f;
vec4 toAxisAngle(const quat& q) {
vec4 res;
float sa = (float) sqrt(1.0f - q.w * q.w);
if (sa < EPS) {
sa = 1.0f;
} else {
sa = 1.0f / sa;
}
res[0] = (float) acos(q.w) * 2.0f;
res[1] = q.x * sa;
res[2] = q.y * sa;
res[3] = q.z * sa;
return res;
}
Related
I need to rotate bones of skeleton, i have already the quaterinion corresponding for each joints; and i am confused when it comes on rotating.
Skeleton to move is my opengl scene i need to move.
My problem is that i can't rotate the joint; Can anyone Help
Bellow is my code
//i evaluate each joint to get the translation and rotation.
void Node::EvaluatePoi(std::vector<POI> pois, const Vector &par_pos,
const Quaternion &par_rot, Vector Node::*world_pos,std::vector<Arti> joints)
{ Vector poi=this->PoiVec ;
Quaternion rot;
if (pois.empty()){
this->*world_pos= this->rest_position ;//OFFSET
rot= this-> rest_rotation ;//identity
}else{
if(this->name=="Hips")
{
this->*world_pos = eval_instant_positionPOI(poi);
rot= this-> rest_rotation ;// do not rotate
}else if(this->name=="LeftUpLeg")
{
this->*world_pos = this->rest_position;// set to OFFSET
rot= this-> rest_rotation ;// do not rotate
}else if(this->name=="RightUpLeg")
{
this->*world_pos = this->rest_position;
rot= this-> rest_rotation ;
}
else
{
this->*world_pos= this->rest_position;
rot= eval_instant_rotationPOI(joints);
}
}
//Applying transformation on the global position with rot =qparent * qchild
(this->*world_pos).rotate(par_rot);
this->*world_pos += par_pos;
rot = par_rot * rot;
// draw joint's subhierarchy
for (int i = 0; i < n_children; i++)
child[i]->EvaluatePoi(pois, this->*world_pos, rot, world_pos,joints);
}
EDIT:
//here i get the local rotation of each joint, after that create quaternions equivalent to individual Euler rotations and then compose to one rotation
Vector x_vector(1.0, 0.0, 0.0),
y_vector(0.0, 1.0, 0.0),
z_vector(0.0, 0.0, 1.0);
Quaternion Node::eval_instant_rotationPOI( std::vector<Arti> joints)
{
Quaternion roto;//= new Quaternion();
Quaternion sample;
double t= 0.02;
Vector v;
Vector Euler(0,0,0);;
string x =this->name;
if(x== "Head"){
Euler=GetEulers(joints,JOINT_HEAD);
}else if(x== "Neck"){
Euler=GetEulers(joints,JOINT_NECK);
}
else if(x== "LeftUpArm"){
Euler=GetEulers(joints,JOINT_LEFT_SHOULDER);
}
else if(x== "RightUpArm"){
Euler=GetEulers(joints,JOINT_RIGHT_SHOULDER);
}
else if(x== "LeftLowArm"){
Euler=GetEulers(joints,JOINT_LEFT_ELBOW);
}
else if(x== "LeftHand"){
Euler=GetEulers(joints,JOINT_LEFT_HAND);
}
else if(x== "RightLowArm"){
Euler=GetEulers(joints,JOINT_RIGHT_ELBOW);
}
else if(x== "RightHand"){
Euler=GetEulers(joints,JOINT_RIGHT_HAND);
}
else if(x== "Hips"){
Euler=GetEulers(joints,JOINT_TORSO);
}
else if(x== "LeftUpLeg"){
Euler=GetEulers(joints,JOINT_LEFT_HIP);
}
else if(x== "RightUpLeg"){
Euler=GetEulers(joints,JOINT_RIGHT_HIP);
}
else if(x== "LeftLowLeg"){
Euler=GetEulers(joints,JOINT_LEFT_KNEE);
}
else if(x== "LeftFoot"){
Euler=GetEulers(joints,JOINT_LEFT_FOOT);
}
else if(x== "RightLowLeg"){
Euler=GetEulers(joints,JOINT_RIGHT_KNEE);
}
else if(x== "RightFoot"){
Euler=GetEulers(joints,JOINT_RIGHT_FOOT);
}
Quaternion qx(x_vector, (Euler.x ));
Quaternion qy(y_vector, (Euler.y ));
Quaternion qz(z_vector, (Euler.z ));
sample = qz * qy * qx;
roto= slerp(qTemp, sample, t);
qTemp=roto;
return roto ;
}
/*here i multiply the joint and its parent to get the Euler Angle ; is it necessary to convert to
Euler Angle?/
Vector Node::GetEulers(std::vector<Arti> joints, const int idx) {
// Get the quaternion of its parent.
Quaternion q_parent;
Quaternion q_current;
if (idx == JOINT_TORSO) {
q_parent.identity();
}
/////
{
q_parent = Quaternion(joints[parent_joint_map[idx]].quat.x,
joints[parent_joint_map[idx]].quat.y,
joints[parent_joint_map[idx]].quat.z,
joints[parent_joint_map[idx]].quat.w);
}
// Get the quaternion of the joint.
q_current = Quaternion(joints[idx].quat.x, joints[idx].quat.y,
joints[idx].quat.z, joints[idx].quat.w);
// Calculate the relative quaternion.
Quaternion q_delta = quat_left_multiply(q_current , quat_inverse(q_parent));
Vector angle = euler_from_quat(q_delta);
// cout<<this->name<<" "<<angle<<" ";
return angle;
}
Quaternion quat_left_multiply(Quaternion l, Quaternion r) {
Quaternion q = {r.w * l.x + r.x * l.w + r.y * l.z - r.z * l.y,
r.w * l.y + r.y * l.w + r.z * l.x - r.x * l.z,
r.w * l.z + r.z * l.w + r.x * l.y - r.y * l.x,
r.w * l.w - r.x * l.x - r.y * l.y - r.z * l.z};
return q;
}
Vector& Vector::rotate(const Quaternion& q)
{
Quaternion p(x, y, z, 0.0f);
Quaternion qc(q);
qc.conjugate();
Quaternion pp(q * p * qc);
x = pp.x;
y = pp.y;
z = pp.z;
return *this;
}
Rotating a quaternion is actually multiplying a quaternion by another. Given the quaternion qA representing the current rotation of an object and qB the quaternion representing the amount of rotation to apply (to add) to this object, the resulting new rotation of this object is computed as follow (pseudocode):
qA = qA * qB;
Alternatively, you can apply (add) this rotation in what is called "object" or "local" transformation space by swapping operands:
qA = qB * qA
Each joint should hold (usualy as class member) a quaternion representing its current rotation in local space. This is probably what you already done. If you want to apply a rotation to that joint, then you simply need multiply the joint quaternion, by another quaternion representing the amount of rotation to apply. A quaterion rotation method can be like this (pseudocode):
Joint::Rotate(const quaterion& amount, bool local)
{
if(local) {
this->rotation = amount * this->rotation;
} else {
this->rotation = this->rotation * amount;
}
this->rotation.normalize();
}
That is all you need for the rotation part, nothing else. After that, you will need to convert the joint quaternion to a rotation matrix, so to be combined with the other joint transformations (translation, scale, whatever). Here is one implementation of the quaternion to rotation matrix conversion (pseudocode):
Matrix3 QuaternionToMatrix(const quaternion& q)
{
float x2 = q.x + q.x;
float y2 = q.y + q.y;
float z2 = q.z + q.z;
float xx = q.x * x2;
float xy = q.x * y2;
float xz = q.x * z2;
float yy = q.y * y2;
float yz = q.y * z2;
float zz = q.z * z2;
float wx = q.w * x2;
float wy = q.w * y2;
float wz = q.w * z2;
Matrix3 m; //< 3x3 matrix
m[0] = (1.0f - (yy + zz));
m[1] = (xy - wz);
m[2] = (xz + wy);
m[3] = (xy + wz);
m[4] = (1.0f - (xx + zz));
m[5] = (yz - wx);
m[6] = (xz - wy);
m[7] = (yz + wx);
m[8] = (1.0f - (xx + yy));
return m;
}
What you may finally need is to input rotation using Euler angles instead of quaternion values. Indeed, Euler angles are easier to handle and understand when it come to apply a rotation in a human point of view. In this case, you'll need to convert the input Euler angles to a quaternion. Here is one possible implementation of Euler angle to quaternion conversion:
Quaternion EulerToQuaternion(float x, float y, float z)
{
float sx = sinf(x * -0.5f);
float cx = cosf(x * -0.5f);
float sy = sinf(y * -0.5f);
float cy = cosf(y * -0.5f);
float sz = sinf(z * -0.5f);
float cz = cosf(z * -0.5f);
Quaternion q;
q.x = sx * cy * cz + cx * sy * sz;
q.y = cx * sy * cz - sx * cy * sz;
q.z = cx * cy * sz + sx * sy * cz;
q.w = cx * cy * cz - sx * sy * sz;
return q;
}
We are trying to convert from a local space incremental rotation in Euler (X,Y,Z) = Pitch Yaw Roll to an absolute world space rotation Quaternion.
We are doing this by converting each incremental rotation to a quaternion and accumulating (through multiplication) Quaternion rotations to give a world space quaternion.
However our result rotations show the object rotating around world space axes rather than local object axes.
I am following this pseudocode loop and c++, and have built the same in Unity3D (which works correctly since the Quaternion operations are aready provided).
Does anyone have any pointers as to what is going wrong here?
Quaternion qacc;
Quaternion q1;
loop
{
quacc = quacc * q1.degreeToQuaternion(xRot, yRot, zRot);
}
...
void Quaternion::degreeToQuaternion( double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
yaw = yaw * M_PI / 180.;
pitch = pitch * M_PI / 180.;
roll = roll * M_PI / 180.;
radianToQuaternion(yaw, pitch, roll);
}
void Quaternion::radianToQuaternion( double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
this->w = cy * cp * cr + sy * sp * sr;
this->x = cy * cp * sr - sy * sp * cr;
this->y = sy * cp * sr + cy * sp * cr;
this->z = sy * cp * cr - cy * sp * sr;
}
Quaternion operator * (Quaternion q0, Quaternion q1)
{
Quaternion q2;
double mag;
q2.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
q2.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
q2.y = q0.w * q1.y - q0.x * q1.z + q0.y * q1.w + q0.z * q1.x;
q2.z = q0.w * q1.z + q0.x * q1.y - q0.y * q1.x + q0.z * q1.w;
mag = sqrt (q2.w * q2.w + q2.x * q2.x + q2.y * q2.y + q2.z * q2.z);
q2.w /= mag;
q2.x /= mag;
q2.y /= mag;
q2.z /= mag;
return q2;
}
My plan:
1. Calculate mouse direction [x, y] [success]
I my Mouse Move event:
int directionX = lastPosition.x - position.x;
int directionY = lastPosition.y - position.y;
2. Calculate angles [theta, phi] [success]
float theta = fmod(lastTheta + sensibility * directionY, M_PI);
float phi = fmod(lastPhi + sensibility * directionX * -1, M_PI * 2);
Edit {
bug fix:
float theta = lastTheta + sensibility * directionY * -1;
if (theta < M_PI / -2)theta = M_PI / -2;
else if (theta > M_PI / 2)theta = M_PI / 2;
float phi = fmod(lastPhi + sensibility * directionX * -1, M_PI * 2);
}
Now I have given theta, phi, the centerpoint and the radius and I want to calculate the position and the rotation [that the camera look at the centerpoint]
3. Calculate position coordinates [X,Y,Z] [failed]
float newX = radius * sin(phi) * cos(theta);
float newY = radius * sin(phi) * sin(theta);
float newZ = radius * cos(phi);
Solution [by meowgoesthedog]:
float newX = radius * cos(theta) * cos(phi);
float newY = radius * sin(theta);
float newZ = radius * cos(theta) * sin(phi);
4. Calculate rotation [failed]
float pitch = ?;
float yaw = ?;
Solution [by meowgoesthedog]:
float pitch = -theta;
float yaw = -phi;
Thanks for your solutions!
Your attempt was almost (kinda) correct:
As the diagram shows, in OpenGL the "vertical" direction is conventionally taken to be Y, whereas your formulas assume it is Z
phi and theta are in the wrong order
Very simple conversion: yaw = -phi, pitch = -theta (from the perspective of the camera)
Fixed formulas:
float position_X = radius * cos(theta) * cos(phi);
float position_Y = radius * sin(theta);
float position_Z = radius * cos(theta) * sin(phi);
(There may also be some sign issues with the mouse deltas but they should be easy to fix.)
I am given a Line PQ and a boundary. I have to find two parallel lines to the given line but lines should intersect the boundary. Also i know the distance between the parallel lines to the given line. I need to find the P'Q' and P"Q".
Please any one give a simple solution.
float vx = x2 - x1;
float vy = y2 - y1;
float mag = sqrt(vx * vx + vy * vy);
float t = (mag / 2.0) / mag;
float px = (1 - t) * x1 + t * x2;
float py = (1 - t) * y1 + t * y2;
I just found the centre point of PQ by the above code. Further i planned to draw a perpendicular line thru (px, py) with the known distance, then drawing lines perpendicular to that new line(those lines will be parallel to PQ), thru the end points of that new line. But i could not achieve it. can anyone help me or suggest me a way who know maths
Finally i got the solution.
The steps are.
First i am getting the center point of PQ.
POINT find_a_point_in_distance(float x1, float y1, float x2, float y2, float len = 0) {
float vx = x2 - x1;
float vy = y2 - y1;
float mag = sqrt(vx * vx + vy * vy);
float t = len == 0 ? ((mag / 2.0) / mag) : (len / mag);
float px = (1 - t) * x1 + t * x2;
float py = (1 - t) * y1 + t * y2;
POINT res = { px, py };
return res;
}
here (px, py) is center point of PQ.
Then i am finding the perpendicular line through (px, py).
Already i have mentioned in the question that i know distance between PQ and P'Q' also PQ and P"Q". So i am getting two points in that perpendicular line using that distance. Finally i know the angle of the line PQ, so P'Q' and P"Q" should be in that same angle, using these details i can get the lines P'Q' and P"Q" what ever length i want. Here in below code i am getting the line P'Q' and P"Q" with the length of the diagonal of the rectangular box.
POINT res = find_a_point_in_distance(x1, y1, x2, y2);
POINT res2 = find_a_point_in_distance(res.x, res.y, x2, y2, halflen);
float cosA = acos((res2.x - res.x) / halflen) * 180 / PI;
float sinA = asin((res2.y - res.y) / halflen) * 180 / PI;
float cosAngle = cos((cosA + 90.0) * PI / 180.0);
float sinAngle = sin((sinA + 90.0) * PI / 180.0);
float cx1 = res.x + halflen * cosAngle;
float cy1 = res.y + halflen * sinAngle;
float cosAngle2 = cos((cosA - 90.0) * PI / 180.0);
float sinAngle2 = sin((sinA - 90.0) * PI / 180.0);
float cx2 = res.x + halflen * cosAngle2;
float cy2 = res.y + halflen * sinAngle2;
float diagonal = sqrt(width * width + height * height);
float halfdiagonal = diagonal / 2.0;
float cosAngleT = cos(cosA * PI / 180.0);
float sinAngleT = sin(sinA * PI / 180.0);
float cosAngleTD = cos((cosA + 180) * PI / 180.0);
float sinAngleTD = sin((sinA + 180) * PI / 180.0);
float cx10 = cx1 + halfdiagonal * cosAngleT;
float cy10 = cy1 + halfdiagonal * sinAngleT;
float cx11 = cx1 + halfdiagonal * cosAngleTD;
float cy11 = cy1 + halfdiagonal * sinAngleTD;
float cx20 = cx2 + halfdiagonal * cosAngleT;
float cy20 = cy2 + halfdiagonal * sinAngleT;
float cx21 = cx2 + halfdiagonal * cosAngleTD;
float cy21 = cy2 + halfdiagonal * sinAngleTD;
here (cx10, cy10) and (cx11, cy11) is line P'Q' and
(cx20, cy20) and (cx21, cy21) is line P"Q".
then finally im finding the intersect point of P'Q' and P"Q" with all sides of rectangle
I want to draw a sphere using VBO for vertex, color and UV coordinates for texture. My problem is that the sphere is not 'closed', there is a hole in the origin. I know that this is because my code depends on (1/segments) distance between each vertex; I am working with segments = 40.
I know that, if I rise that value, the hole will be lower, but program is slower. I don't know if there's a way to eliminate the hole without rise the variable.
Here's the code:
for(int i = 0; i <= segments; i++){
double lat0 = pi * (-0.5 + (double)(i - 1) / segments);
double z0 = sin(lat0);
double zr0 = cos(lat0);
// lat1 = [-pi/2..pi/2]
double lat1 = pi * (-0.5 + (double)i / segments);
double z1 = sin(lat1);
double zr1 = cos(lat1);
for (int j = 0; j <= segments; j++){ // Longitud
// lng = [0..2*pi]
double lng = 2 * pi * (double)(j - 1) / segments;
double x = cos(lng);
double y = sin(lng);
//glNormal3f(x * zr0, y * zr0, z0); // Normals
ballVerts.push_back(x * zr0); //X
ballVerts.push_back(y * zr0); //Y
ballVerts.push_back(z0); //Z
ballVerts.push_back(0.0f);
ballVerts.push_back(0.0f);
ballVerts.push_back(0.0f);
ballVerts.push_back(1.0f); //R,G,B,A
texX = abs(1 - (0.5f + atan2(z0, x * zr0) / (2.0 * pi)));
texY = 0.5f - asin(y * zr0) / pi;
ballVerts.push_back(texX); // Texture coords
ballVerts.push_back(texY); // U, V
//glNormal3f(x * zr1, y * zr1, z1); //Normals
ballVerts.push_back(x * zr1); //X
ballVerts.push_back(y * zr1); //Y
ballVerts.push_back(z1); //Z
ballVerts.push_back(0.0f);
ballVerts.push_back(0.0f);
ballVerts.push_back(1.0f);
ballVerts.push_back(1.0f); //R,G,B,A
texX = abs(1 - (0.5f + atan2(z1, x * zr1) / (2.0 * pi)));
texY = 0.5f - asin(y * zr1) / pi;
ballVerts.push_back(texX); // Texture coords
ballVerts.push_back(texY);
}
}
// Create VBO....
And this is the output I have:
I don't think that's a hole. You're drawing one segment too many, and causing it to draw additional triangles at the south pole, with the texture wrapped around:
for(int i = 0; i <= segments; i++){
double lat0 = pi * (-0.5 + (double)(i - 1) / segments);
In the first loop iteration, with i = 0, the angle will be less than -0.5 * pi, resulting in the extra triangles shown in your picture.
If you want to split the latitude range into segments pieces, you only need to run through the outer loop segments times. With the code above, with the loop from 0 up to and including segments, you're iterating segments + 1 times.
The easiest way to fix this is to start the loop at 1:
for(int i = 1; i <= segments; i++){
double lat0 = pi * (-0.5 + (double)(i - 1) / segments);
I would probably loop from 0 and make the end exclusive, and change the angle calculations. But that's really equivalent:
for(int i = 0; i < segments; i++){
double lat0 = pi * (-0.5 + (double) / segments);
...
double lat1 = pi * (-0.5 + (double)(i + 1) / segments);