text:
/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef math_870b373b_c3fb_4c7a_abc7_bc4bd4095aef_h
#define math_870b373b_c3fb_4c7a_abc7_bc4bd4095aef_h

#include <gslib/config.h>
#include <gslib/basetype.h>

#ifndef PI
#define PI  (3.14159265358979323846f)
#endif

__gslib_begin__

struct _vec2 { float x, y; };
struct _vec3 { float x, y, z; };
struct _vec4 { float x, y, z, w; };
struct _mat2
{
    union
    {
        struct
        {
            float _11, _12,
                _21, _22;
        };
        float  m[2][2];
    };
};
struct _mat3
{
    union
    {
        struct
        {
            float _11, _12, _13,
                _21, _22, _23,
                _31, _32, _33;
        };
        float   m[3][3];
    };
};
struct _mat4
{
    union
    {
        struct
        {
            float _11, _12, _13, _14,
                _21, _22, _23, _24,
                _31, _32, _33, _34,
                _41, _42, _43, _44;
        };
        float   m[4][4];
    };
};
struct _quat
{
    union
    {
        struct  { float x, y, z, w; };
        float   q[4];
    };
};
struct _plane { float a, b, c, d; };

struct viewport
{
    int         left;
    int         top;
    uint        width;
    uint        height;
    float       min_depth;
    float       max_depth;
};

class vec2;
class vec3;
class vec4;
class mat3;
class mat4;
class quat;
class plane;

typedef vec2 point2;
typedef vec3 point3;
typedef mat4 matrix;
typedef mat3 matrix3;
typedef _mat2 matrix2;

/* inline */
float vec2length(const vec2* p);
float vec2lengthsq(const vec2* p);
float vec2dot(const vec2* p1, const vec2* p2);
float vec2ccw(const vec2* p1, const vec2* p2);
vec2* vec2add(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2sub(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2min(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2max(vec2* out, const vec2* p1, const vec2* p2);
vec2* vec2scale(vec2* out, const vec2* p, float s);
vec2* vec2lerp(vec2* out, const vec2* p1, const vec2* p2, float s);
float vec2lerp_x(const vec2* p1, const vec2* p2, float y);
float vec2lerp_y(const vec2* p1, const vec2* p2, float x);
vec2* vec2multiply(vec2* out, const vec2* p, const matrix2* m);
vec2* vec2transformcoord(vec2* out, const vec2* p, const mat3* m);
vec2* vec2transformnormal(vec2* out, const vec2* p, const mat3* m);
float vec3length(const vec3* v);
float vec3lengthsq(const vec3* v);
float vec3dot(const vec3* v1, const vec3* v2);
vec3* vec3cross(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3add(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3sub(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3min(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3max(vec3* out, const vec3* v1, const vec3* v2);
vec3* vec3scale(vec3* out, const vec3* v, float s);
vec3* vec3lerp(vec3* out, const vec3* v1, const vec3* v2, float s);
vec3* vec3multiply(vec3* out, const vec3* v, const mat3* m);
vec3* vec3project(vec3* out, const vec3* v, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world);
vec3* vec3unproject(vec3* out, const vec3* v, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world);
vec3* vec3projectarray(vec3* out, uint ostride, const vec3* v, uint vstride, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world, uint n);
vec3* vec3unprojectarray(vec3* out, uint ostride, const vec3* v, uint vstride, const viewport* vp, const matrix* prj, const matrix* view, const matrix* world, uint n);
float vec4length(const vec4* v);
float vec4lengthsq(const vec4* v);
float vec4dot(const vec4* v1, const vec4* v2);
vec4* vec4add(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4sub(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4min(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4max(vec4* out, const vec4* v1, const vec4* v2);
vec4* vec4scale(vec4* out, const vec4* v, float s);
vec4* vec4lerp(vec4* out, const vec4* v1, const vec4* v2, float s);
vec4* vec4multiply(vec4* out, const vec4* v, const matrix* m);
matrix3* mat3identity(matrix3* out);
bool mat3isidentity(const matrix3* m);
float mat3determinant(const matrix3* m);
matrix3* mat3inverse(matrix3* out, float* det, const matrix3* m);
matrix3* mat3multiply(matrix3* out, const matrix3* m1, const matrix3* m2);
matrix3* mat3transpose(matrix3* out, const matrix3* m);
matrix3* mat3scaling(matrix3* out, float sx, float sy);
matrix3* mat3translation(matrix3* out, float x, float y);
matrix3* mat3rotation(matrix3* out, float angle);
matrix3* mat3rotationpos(matrix3* out, const vec2* p, float angle);
matrix* matidentity(matrix* out);
bool matisidentity(const matrix* m);
bool matdecompose(vec3* scale, quat* rot, vec3* trans, const matrix* m);
matrix* mattranspose(matrix* out, const matrix* m);
matrix* matscaling(matrix* out, float sx, float sy, float sz);
matrix* mattranslation(matrix* out, float x, float y, float z);
matrix* matrotation_x(matrix* out, float angle);
matrix* matrotation_y(matrix* out, float angle);
matrix* matrotation_z(matrix* out, float angle);
matrix* matrotationaxis(matrix* out, const vec3* v, float angle);
matrix* matrotationquatern(matrix* out, const quat* q);
matrix* matrotationeuler(matrix* out, float yaw, float pitch, float roll);
matrix* matlookatlh(matrix* out, const vec3* eye, const vec3* at, const vec3* up);
matrix* matlookatrh(matrix* out, const vec3* eye, const vec3* at, const vec3* up);
matrix* matperspectivelh(matrix* out, float w, float h, float zn, float zf);
matrix* matperspectiverh(matrix* out, float w, float h, float zn, float zf);
matrix* matperspectivefovlh(matrix* out, float fovy, float aspect, float zn, float zf);
matrix* matperspectivefovrh(matrix* out, float fovy, float aspect, float zn, float zf);
matrix* matperspectiveoffcenterlh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matperspectiveoffcenterrh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matortholh(matrix* out, float w, float h, float zn, float zf);
matrix* matorthorh(matrix* out, float w, float h, float zn, float zf);
matrix* matorthooffcenterlh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matorthooffcenterrh(matrix* out, float l, float r, float b, float t, float zn, float zf);
matrix* matviewportproject(matrix* out, const viewport* vp);
matrix* matviewportunproject(matrix* out, const viewport* vp);
matrix* mattransform(matrix* out, const vec3* scalecenter, const quat* scalerot, const vec3* scale, const vec3* rotcenter, const quat* rot, const vec3* trans);
matrix* mattransform2d(matrix* out, const vec2* scalecenter, float scalerot, const vec2* scale, const vec2* rotcenter, float rot, const vec2* trans);
matrix* mataffinetransform(matrix* out, float scale, const vec3* rotcenter, const quat* rot, const vec3* trans);
matrix* mataffinetransform2d(matrix* out, float scale, const vec2* rotcenter, float rot, const vec2* trans);
float quatlength(const quat* q);
float quatlengthsq(const quat* q);
float quatdot(const quat* q1, const quat* q2);
quat* quatidentity(quat* out);
bool quatisidenetity(const quat* q);
quat* quatconjugate(quat* out, const quat* q);
void quattoaxisangle(const quat* q, vec3* axis, float* angle);
quat* quatrotatematrix(quat* out, const matrix* m);
quat* quatrotateaxis(quat* out, const vec3* v, float angle);
quat* quatln(quat* out, const quat* q);
quat* quatexp(quat* out, const quat* q);
quat* quatslerp(quat* out, const quat* q1, const quat* q2, float t);
quat* quatsquad(quat* out, const quat* q, const quat* a, const quat* b, const quat* c, float t);
quat* quatbarycentric(quat* out, const quat* q1, const quat* q2, const quat* q3, float f, float g);
void quatsquadsetup(quat* a, quat* b, quat* c, const quat* q1, const quat* q2, const quat* q3, const quat* q4);
float planedot(const plane* p, const vec4* v);
float planedotcoord(const plane* p, const vec3* v);
float planedotnormal(const plane* p, const vec3* v);
plane* planescale(plane* out, const plane* p, float s);
plane* planefrompointnormal(plane* out, const vec3* p, const vec3* normal);

/* non-inline */
typedef vec2* (__stdcall *fnvec2normalize)(vec2* out, const vec2* v);
typedef vec2* (__stdcall *fnvec2hermite)(vec2* out, const vec2* p1, const vec2* t1, const vec2* p2, const vec2* t2, float s);
typedef vec2* (__stdcall *fnvec2catmullrom)(vec2* out, const vec2* p1, const vec2* p2, const vec2* p3, const vec2* p4, float s);
typedef vec2* (__stdcall *fnvec2barycentric)(vec2* out, const vec2* p1, const vec2* p2, const vec2* p3, float f, float g);
typedef vec4* (__stdcall *fnvec2transform)(vec4* out, const vec2* p, const matrix* m);
typedef vec2* (__stdcall *fnvec2transformcoord)(vec2* out, const vec2* p, const matrix* m);
typedef vec2* (__stdcall *fnvec2transformnormal)(vec2* out, const vec2* p, const matrix* m);
typedef vec4* (__stdcall *fnvec2transformarray)(vec4* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec2* (__stdcall *fnvec2transformcoordarray)(vec2* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec2* (__stdcall *fnvec2transformnormalarray)(vec2* out, uint ostride, const vec2* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3normalize)(vec3* out, const vec3* v);
typedef vec3* (__stdcall *fnvec3hermite)(vec3* out, const vec3* v1, const vec3* t1, const vec3* v2, const vec3* t2, float s);
typedef vec3* (__stdcall *fnvec3catmullrom)(vec3* out, const vec3* v1, const vec3* v2, const vec3* v3, const vec3* v4, float s);
typedef vec3* (__stdcall *fnvec3barycentric)(vec3* out, const vec3* v1, const vec3* v2, const vec3* v3, float f, float g);
typedef vec4* (__stdcall *fnvec3transform)(vec4* out, const vec3* v, const matrix* m);
typedef vec3* (__stdcall *fnvec3transformcoord)(vec3* out, const vec3* v, const matrix* m);
typedef vec3* (__stdcall *fnvec3transformnormal)(vec3* out, const vec3* v, const matrix* m);
typedef vec4* (__stdcall *fnvec3transformarray)(vec4* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3transformcoordarray)(vec3* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec3* (__stdcall *fnvec3transformnormalarray)(vec3* out, uint ostride, const vec3* v, uint vstride, const matrix* m, uint n);
typedef vec4* (__stdcall *fnvec4cross)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3);
typedef vec4* (__stdcall *fnvec4normalize)(vec4* out, const vec4* v);
typedef vec4* (__stdcall *fnvec4hermite)(vec4* out, const vec4* v1, const vec4* t1, const vec4* v2, const vec4* t2, float s);
typedef vec4* (__stdcall *fnvec4catmullrom)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3, const vec4* v4, float s);
typedef vec4* (__stdcall *fnvec4barycentric)(vec4* out, const vec4* v1, const vec4* v2, const vec4* v3, float f, float g);
typedef vec4* (__stdcall *fnvec4transform)(vec4* out, const vec4* v, const matrix* m);
typedef vec4* (__stdcall *fnvec4transformarray)(vec4* out, uint ostride, const vec4* v, uint vstride, const matrix* m, uint n);
typedef float (__stdcall *fnmatdeterminant)(const matrix* m);
typedef matrix* (__stdcall *fnmatmultiply)(matrix* out, const matrix* m1, const matrix* m2);
typedef matrix* (__stdcall *fnmatmultiplytranspose)(matrix* out, const matrix* m1, const matrix* m2);
typedef matrix* (__stdcall *fnmatinverse)(matrix* out, float* determinant, const matrix* m);
typedef matrix* (__stdcall *fnmatshadow)(matrix* out, const vec4* plight, const plane* pln);
typedef matrix* (__stdcall *fnmatreflect)(matrix* out ,const plane* pln);
typedef quat* (__stdcall *fnquatrotateeuler)(quat* out, float yaw, float pitch, float roll);
typedef quat* (__stdcall *fnquatmultiply)(quat* out, const quat* q1, const quat* q2);
typedef quat* (__stdcall *fnquatnormalize)(quat* out, const quat* q);
typedef quat* (__stdcall *fnquatinverse)(quat* out, const quat* q);
typedef plane* (__stdcall *fnplanenormalize)(plane* out, const plane* p);
typedef vec3* (__stdcall *fnplaneintersectline)(vec3* out, const plane* p, const vec3* v1, const vec3* v2);
typedef plane* (__stdcall *fnplanefrompoints)(plane* out, const vec3* v1, const vec3* v2, const vec3* v3);
typedef plane* (__stdcall *fnplanetransform)(plane* out, const plane* p, const matrix* m);
typedef plane* (__stdcall *fnplanetransformarray)(plane* out, uint ostride, const plane* p, uint pstride, const matrix* m, uint n);

gs_export extern fnvec2normalize vec2normalize;
gs_export extern fnvec2hermite vec2hermite;
gs_export extern fnvec2catmullrom vec2catmullrom;
gs_export extern fnvec2barycentric vec2barycentric;
gs_export extern fnvec2transform vec2transform;
gs_export extern fnvec2transformarray vec2transformarray;
gs_export extern fnvec2transformcoordarray vec2transformcoordarray;
gs_export extern fnvec2transformnormalarray vec2transformnormalarray;
gs_export extern fnvec3normalize vec3normalize;
gs_export extern fnvec3hermite vec3hermite;
gs_export extern fnvec3catmullrom vec3catmullrom;
gs_export extern fnvec3barycentric vec3barycentric;
gs_export extern fnvec3transform vec3transform;
gs_export extern fnvec3transformcoord vec3transformcoord;
gs_export extern fnvec3transformnormal vec3transformnormal;
gs_export extern fnvec3transformarray vec3transformarray;
gs_export extern fnvec3transformcoordarray vec3transformcoordarray;
gs_export extern fnvec3transformnormalarray vec3transformnormalarray;
gs_export extern fnvec4cross vec4cross;
gs_export extern fnvec4normalize vec4normalize;
gs_export extern fnvec4hermite vec4hermite;
gs_export extern fnvec4catmullrom vec4catmullrom;
gs_export extern fnvec4barycentric vec4barycentric;
gs_export extern fnvec4transform vec4transform;
gs_export extern fnvec4transformarray vec4transformarray;
gs_export extern fnmatdeterminant matdeterminant;
gs_export extern fnmatmultiply matmultiply;
gs_export extern fnmatmultiplytranspose matmultiplytranspose;
gs_export extern fnmatinverse matinverse;
gs_export extern fnmatshadow matshadow;
gs_export extern fnmatreflect matreflect;
gs_export extern fnquatrotateeuler quatrotateeuler;
gs_export extern fnquatmultiply quatmultiply;
gs_export extern fnquatnormalize quatnormalize;
gs_export extern fnquatinverse quatinverse;
gs_export extern fnplanenormalize planenormalize;
gs_export extern fnplaneintersectline planeintersectline;
gs_export extern fnplanefrompoints planefrompoints;
gs_export extern fnplanetransform planetransform;
gs_export extern fnplanetransformarray planetransformarray;

class gs_export vec2:
    public _vec2
{
public:
    vec2() {}
    vec2(const float*);
    vec2(float x, float y);

public:
    operator float*();
    operator const float*() const;

public:
    vec2& operator+=(const vec2&);
    vec2& operator-=(const vec2&);
    vec2& operator*=(float);
    vec2& operator/=(float);

public:
    vec2 operator+() const;
    vec2 operator-() const;

public:
    vec2 operator+(const vec2&) const;
    vec2 operator-(const vec2&) const;
    vec2 operator*(float) const;
    vec2 operator/(float) const;
    friend vec2 operator*(float, const vec2&);

public:
    bool operator==(const vec2&) const;
    bool operator!=(const vec2&) const;

public:
    float length() const { return vec2length(this); }
    float lengthsq() const { return vec2lengthsq(this); }
    float dot(const vec2& v) const { return vec2dot(this, &v); }
    float ccw(const vec2& v) const { return vec2ccw(this, &v); }
    vec2& add(const vec2& p1, const vec2& p2) { return *vec2add(this, &p1, &p2); }
    vec2& sub(const vec2& p1, const vec2& p2) { return *vec2sub(this, &p1, &p2); }
    vec2& scale(float s) { return *vec2scale(this, this, s); }
    vec2& scale(const vec2& v, float s) { return *vec2scale(this, &v, s); }
    vec2& lerp(const vec2& p1, const vec2& p2, float s) { return *vec2lerp(this, &p1, &p2, s); }
    vec2& multiply(const matrix2& m) { return *vec2multiply(this, this, &m); }
    vec2& multiply(const vec2& p, const matrix2& m) { return *vec2multiply(this, &p, &m); }
    vec2& normalize() { return *vec2normalize(this, this); }
    vec2& normalize(const vec2& v) { return *vec2normalize(this, &v); }
    vec2& hermite(const vec2& p1, const vec2& t1, const vec2& p2, const vec2& t2, float s) { return *vec2hermite(this, &p1, &t1, &p2, &t2, s); }
    vec2& catmullrom(const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4, float s) { return *vec2catmullrom(this, &p1, &p2, &p3, &p4, s); }
    vec2& barycentric(const vec2& p1, const vec2& p2, const vec2& p3, float f, float g) { return *vec2barycentric(this, &p1, &p2, &p3, f, g); }
    vec4& transform(vec4& v, const matrix& m) const { return *vec2transform(&v, this, &m); }
    vec2& transformcoord(const mat3& m) { return *vec2transformcoord(this, this, &m); }
    vec2& transformcoord(const vec2& p, const mat3& m) { return *vec2transformcoord(this, &p, &m); }
    vec2& transformnormal(const mat3& m) { return *vec2transformnormal(this, this, &m); }
    vec2& transformnormal(const vec2& p, const mat3& m) { return *vec2transformnormal(this, &p, &m); }
};

class gs_export vec3:
    public _vec3
{
public:
    vec3() {}
    vec3(const float*);
    vec3(float x, float y, float z);

public:
    operator float*();
    operator const float*() const;

public:
    vec3& operator+=(const vec3&);
    vec3& operator-=(const vec3&);
    vec3& operator*=(float);
    vec3& operator/=(float);

public:
    vec3 operator+() const;
    vec3 operator-() const;

public:
    vec3 operator+(const vec3&) const;
    vec3 operator-(const vec3&) const;
    vec3 operator*(float) const;
    vec3 operator/(float) const;
    friend vec3 operator*(float, const vec3&);

public:
    bool operator==(const vec3&) const;
    bool operator!=(const vec3&) const;

public:
    float length() const { return vec3length(this); }
    float lengthsq() const { return vec3lengthsq(this); }
    float dot(const vec3& v) const { return vec3dot(this, &v); }
    vec3& add(const vec3& v1, const vec3& v2) { return *vec3add(this, &v1, &v2); }
    vec3& sub(const vec3& v1, const vec3& v2) { return *vec3sub(this, &v1, &v2); }
    vec3& cross(const vec3& v) { return *vec3cross(this, &vec3(*this), &v); }
    vec3& cross(const vec3& v1, const vec3& v2) { return *vec3cross(this, &v1, &v2); }
    vec3& scale(float s) { return *vec3scale(this, this, s); }
    vec3& scale(const vec3& v, float s) { return *vec3scale(this, &v, s); }
    vec3& lerp(const vec3& p1, const vec3& p2, float s) { return *vec3lerp(this, &p1, &p2, s); }
    vec3& multiply(const vec3& p, const mat3& m) { return *vec3multiply(this, &p, &m); }
    vec3& project(const viewport& vp, const matrix& prj, const matrix& view, const matrix& world) { return *vec3project(this, this, &vp, &prj, &view, &world); }
    vec3& unproject(const viewport& vp, const matrix& prj, const matrix& view, const matrix& world) { return *vec3unproject(this, this, &vp, &prj, &view, &world); }
    vec3& normalize() { return *vec3normalize(this, this); }
    vec3& normalize(const vec3& v) { return *vec3normalize(this, &v); }
    vec3& hermite(const vec3& v1, const vec3& t1, const vec3& v2, const vec3& t2, float s) { return *vec3hermite(this, &v1, &t1, &v2, &t2, s); }
    vec3& catmullrom(const vec3& v1, const vec3& v2, const vec3& v3, const vec3& v4, float s) { return *vec3catmullrom(this, &v1, &v2, &v3, &v4, s); }
    vec3& barycentric(const vec3& v1, const vec3& v2, const vec3& v3, float f, float g) { return *vec3barycentric(this, &v1, &v2, &v3, f, g); }
    vec4& transform(vec4& v, const matrix& m) const { return *vec3transform(&v, this, &m); }
    vec3& transformcoord(const matrix& m) { return *vec3transformcoord(this, this, &m); }
    vec3& transformcoord(const vec3& v, const matrix& m) { return *vec3transformcoord(this, &v, &m); }
    vec3& transformnormal(const matrix& m) { return *vec3transformnormal(this, this, &m); }
    vec3& transformnormal(const vec3& v, const matrix& m) { return *vec3transformnormal(this, &v, &m); }
};

class gs_export vec4:
    public _vec4
{
public:
    vec4() {}
    vec4(const float*);
    vec4(const _vec3& xyz, float w);
    vec4(float x, float y, float z, float w);

public:
    operator float*();
    operator const float*() const;

public:
    vec4& operator+=(const vec4&);
    vec4& operator-=(const vec4&);
    vec4& operator*=(float);
    vec4& operator/=(float);

public:
    vec4 operator+() const;
    vec4 operator-() const;

public:
    vec4 operator+(const vec4&) const;
    vec4 operator-(const vec4&) const;
    vec4 operator*(float) const;
    vec4 operator/(float) const;
    friend vec4 operator*(float, const vec4&);

public:
    bool operator==(const vec4&) const;
    bool operator!=(const vec4&) const;

public:
    float length() const { return vec4length(this); }
    float lengthsq() const { return vec4lengthsq(this); }
    float dot(const vec4& v) const { return vec4dot(this, &v); }
    vec4& add(const vec4& v1, const vec4& v2) { return *vec4add(this, &v1, &v2); }
    vec4& sub(const vec4& v1, const vec4& v2) { return *vec4sub(this, &v1, &v2); }
    vec4& scale(float s) { return *vec4scale(this, this, s); }
    vec4& scale(const vec4& v, float s) { return *vec4scale(this, &v, s); }
    vec4& lerp(const vec4& v1, const vec4& v2, float s) { return *vec4lerp(this, &v1, &v2, s); }
    vec4& multiply(const matrix& m) { return *vec4multiply(this, this, &m); }
    vec4& multiply(const vec4& v, const matrix& m) { return *vec4multiply(this, &v, &m); }
    vec4& cross(const vec4& v1, const vec4& v2, const vec4& v3) { return *vec4cross(this, &v1, &v2, &v3); }
    vec4& hermite(const vec4& v1, const vec4& t1, const vec4& v2, const vec4& t2, float s) { return *vec4hermite(this, &v1, &t1, &v2, &t2, s); }
    vec4& catmullrom(const vec4& v1, const vec4& v2, const vec4& v3, const vec4& v4, float s) { return *vec4catmullrom(this, &v1, &v2, &v3, &v4, s); }
    vec4& barycentric(const vec4& v1, const vec4& v2, const vec4& v3, float f, float g) { return *vec4barycentric(this, &v1, &v2, &v3, f, g); }
    vec4& transform(const matrix& m) { return *vec4transform(this, this, &m); }
    vec4& transform(const vec4& v, const matrix& m) { return *vec4transform(this, &v, &m); }
};

class gs_export mat3:
    public _mat3
{
public:
    mat3() {}
    mat3(const float*);
    mat3(const mat3&);
    mat3(float f11, float f12, float f13,
         float f21, float f22, float f23,
         float f31, float f32, float f33
         );

public:
    float& operator()(uint row, uint col);
    float operator()(uint row, uint col) const;
    operator float*();
    operator const float*() const;

public:
    mat3& operator*=(const mat3&);
    mat3& operator+=(const mat3&);
    mat3& operator-=(const mat3&);
    mat3& operator*=(float);
    mat3& operator/=(float);

public:
    mat3 operator+() const;
    mat3 operator-() const;

public:
    mat3 operator*(const mat3&) const;
    mat3 operator+(const mat3&) const;
    mat3 operator-(const mat3&) const;
    mat3 operator*(float) const;
    mat3 operator/(float) const;
    friend mat3 operator*(float, const mat3&);

public:
    bool operator==(const mat3&) const;
    bool operator!=(const mat3&) const;

public:
    mat3& identity() { return *mat3identity(this); }
    bool isidentity() const { return mat3isidentity(this); }
    mat3& transpose() { return *mat3transpose(this, this); }
    mat3& transpose(const mat3& m) { return *mat3transpose(this, &m); }
    mat3& scaling(float sx, float sy) { return *mat3scaling(this, sx, sy); }
    mat3& translation(float x, float y) { return *mat3translation(this, x, y); }
    mat3& rotation(float angle) { return *mat3rotation(this, angle); }
    mat3& rotationpos(const vec2& p, float angle) { return *mat3rotationpos(this, &p, angle); }
    mat3& multiply(const mat3& m) { return *mat3multiply(this, this, &m); }
    mat3& multiply(const mat3& m1, const mat3& m2) { return *mat3multiply(this, &m1, &m2); }
    float determinant() const { return mat3determinant(this); }
    mat3& inverse(float* det, const mat3& m) { return *mat3inverse(this, det, &m); }
};

class gs_export mat4:
    public _mat4
{
public:
    mat4() {}
    mat4(const float*);
    mat4(const mat4&);
    mat4(float f11, float f12, float f13, float f14,
         float f21, float f22, float f23, float f24,
         float f31, float f32, float f33, float f34,
         float f41, float f42, float f43, float f44);

public:
    float& operator()(uint row, uint col);
    float operator()(uint row, uint col) const;
    operator float*();
    operator const float*() const;

public:
    mat4& operator*=(const mat4&);
    mat4& operator+=(const mat4&);
    mat4& operator-=(const mat4&);
    mat4& operator*=(float);
    mat4& operator/=(float);

public:
    mat4 operator+() const;
    mat4 operator-() const;

public:
    mat4 operator*(const mat4&) const;
    mat4 operator+(const mat4&) const;
    mat4 operator-(const mat4&) const;
    mat4 operator*(float) const;
    mat4 operator/(float) const;
    friend mat4 operator*(float, const mat4&);

public:
    bool operator==(const mat4&) const;
    bool operator!=(const mat4&) const;

public:
    mat4& identity() { return *matidentity(this); }
    bool isidentity() const { return matisidentity(this); }
    bool decompose(vec3& scale, quat& rot, vec3& trans) { return matdecompose(&scale, &rot, &trans, this); }
    mat4& transpose() { return *mattranspose(this, this); }
    mat4& transpose(const mat4& m) { return *mattranspose(this, &m); }
    mat4& scaling(float sx, float sy, float sz) { return *matscaling(this, sx, sy, sz); }
    mat4& translation(float x, float y, float z) { return *mattranslation(this, x, y, z); }
    mat4& rotation_x(float angle) { return *matrotation_x(this, angle); }
    mat4& rotation_y(float angle) { return *matrotation_y(this, angle); }
    mat4& rotation_z(float angle) { return *matrotation_z(this, angle); }
    mat4& rotation(const vec3& v, float angle) { return *matrotationaxis(this, &v, angle); }
    mat4& rotation(const quat& q) { return *matrotationquatern(this, &q); }
    mat4& rotation(float yaw, float pitch, float roll) { return *matrotationeuler(this, yaw, pitch, roll); }
    mat4& lookatlh(const vec3& eye, const vec3& at, const vec3& up) { return *matlookatlh(this, &eye, &at, &up); }
    mat4& lookatrh(const vec3& eye, const vec3& at, const vec3& up) { return *matlookatrh(this, &eye, &at, &up); }
    mat4& perspectivelh(float w, float h, float zn, float zf) { return *matperspectivelh(this, w, h, zn, zf); }
    mat4& perspectiverh(float w, float h, float zn, float zf) { return *matperspectiverh(this, w, h, zn, zf); }
    mat4& perspectivefovlh(float fovy, float aspect, float zn, float zf) { return *matperspectivefovlh(this, fovy, aspect, zn, zf); }
    mat4& perspectivefovrh(float fovy, float aspect, float zn, float zf) { return *matperspectivefovrh(this, fovy, aspect, zn, zf); }
    mat4& perspectiveoffcenterlh(float l, float r, float b, float t, float zn, float zf) { return *matperspectiveoffcenterlh(this, l ,r, b, t, zn, zf); }
    mat4& perspectiveoffcenterrh(float l, float r, float b, float t, float zn, float zf) { return *matperspectiveoffcenterlh(this, l ,r, b, t, zn, zf); }
    mat4& ortholh(float w, float h, float zn, float zf) { return *matortholh(this, w, h, zn, zf); }
    mat4& orthorh(float w, float h, float zn, float zf) { return *matorthorh(this, w, h, zn, zf); }
    mat4& orthooffcenterlh(float l, float r, float b, float t, float zn, float zf) { return *matorthooffcenterlh(this, l, r, b, t, zn, zf); }
    mat4& orthooffcenterrh(float l, float r, float b, float t, float zn, float zf) { return *matorthooffcenterrh(this, l, r, b, t, zn, zf); }
    mat4& viewportproject(const viewport& vp) { return *matviewportproject(this, &vp); }
    mat4& viewportunproject(const viewport& vp) { return *matviewportunproject(this, &vp); }
    mat4& transform(const vec3& scalecenter, const quat& scalerot, const vec3& scale, const vec3& rotcenter, const quat& rot, const vec3& trans) { return *mattransform(this, &scalecenter, &scalerot, &scale, &rotcenter, &rot, &trans); }
    mat4& transform2d(const vec2& scalecenter, float scalerot, const vec2& scale, const vec2& rotcenter, float rot, const vec2& trans) { return *mattransform2d(this, &scalecenter, scalerot, &scale, &rotcenter, rot, &trans); }
    mat4& affinetransform(float scale, const vec3& rotcenter, const quat& rot, const vec3& trans) { return *mataffinetransform(this, scale, &rotcenter, &rot, &trans); }
    mat4& affinetransform2d(float scale, const vec2& rotcenter, float rot, const vec2& trans) { return *mataffinetransform2d(this, scale, &rotcenter, rot, &trans); }
    float determinant() const { return matdeterminant(this); }
    mat4& multiply(const mat4& m) { return *matmultiply(this, this, &m); }
    mat4& multiply(const mat4& m1, const mat4& m2) { return *matmultiply(this, &m1, &m2); }
    mat4& multiplytranspose(const mat4& m) { return *matmultiplytranspose(this, this, &m); }
    mat4& multiplytranspose(const mat4& m1, const mat4& m2) { return *matmultiplytranspose(this, &m1, &m2); }
    mat4& inverse(float* determinant, const mat4& m) { return *matinverse(this, determinant, &m); }
    mat4& shadow(const vec4& plight, const plane& pln) { return *matshadow(this, &plight, &pln); }
    mat4& reflect(const plane& pln) { return *matreflect(this, &pln); }
};

class gs_export quat:
    public _quat
{
public:
    quat() {}
    quat(const float*);
    quat(float x, float y, float z, float w);

public:
    operator float*();
    operator const float*() const;

public:
    quat& operator+=(const quat&);
    quat& operator-=(const quat&);
    quat& operator*=(const quat&);
    quat& operator*=(float);
    quat& operator/=(float);

public:
    quat operator+() const;
    quat operator-() const;

public:
    quat operator+(const quat&) const;
    quat operator-(const quat&) const;
    quat operator*(const quat&) const;
    quat operator*(float) const;
    quat operator/(float) const;
    friend quat operator*(float, const quat&);

public:
    bool operator==(const quat&) const;
    bool operator!=(const quat&) const;

public:
    float length() const { return quatlength(this); }
    float lengthsq() const { return quatlengthsq(this); }
    float dot(const quat& q) const { return quatdot(this, &q); }
    quat& identity() { return *quatidentity(this); }
    bool isidentity() const { return quatisidenetity(this); }
    quat& conjugate() { return *quatconjugate(this, this); }
    quat& conjugate(const quat& q) { return *quatconjugate(this, &q); }
    float toaxisangle(vec3& axis) const
    {
        float f;
        quattoaxisangle(this, &axis, &f);
        return f;
    }
    quat& rotate(const matrix& m) { return *quatrotatematrix(this, &m); }
    quat& rotate(const vec3& v, float angle) { return *quatrotateaxis(this, &v, angle); }
    quat& ln() { return *quatln(this, this); }
    quat& ln(const quat& q) { return *quatln(this, &q); }
    quat& exp() { return *quatexp(this, this); }
    quat& exp(const quat& q) { return *quatexp(this, &q); }
    quat& slerp(const quat& q1, const quat& q2, float t) { return *quatslerp(this, &q1, &q2, t); }
    quat& squad(const quat& q, const quat& a, const quat& b, const quat& c, float f) { return *quatsquad(this, &q, &a, &b, &c, f); }
    quat& squad(const quat& a, const quat& b, const quat& c, float f) { return *quatsquad(this, this, &a, &b, &c, f); }
    quat& barycentric(const quat& q1, const quat& q2, const quat& q3, float f, float g) { return *quatbarycentric(this, &q1, &q2, &q3, f, g); }
    quat& rotate(float yaw, float pitch, float roll) { return *quatrotateeuler(this, yaw, pitch, roll); }
    quat& multiply(const quat& q1, const quat& q2) { return *quatmultiply(this, &q1, &q2); }
    quat& normalize() { return *quatnormalize(this, this); }
    quat& normalize(const quat& q) { return *quatnormalize(this, &q); }
    quat& inverse() { return *quatinverse(this, this); }
    quat& inverse(const quat& q) { return *quatinverse(this, &q); }
};

class gs_export plane:
    public _plane
{
public:
    plane() {}
    plane(const float*);
    plane(float a, float b, float c, float d);

public:
    operator float*();
    operator const float*() const;

public:
    plane& operator*=(float);
    plane& operator/=(float);
    plane operator+() const;
    plane operator-() const;
    plane operator*(float) const;
    plane operator/(float) const;
    friend plane operator*(float, const plane&);

public:
    bool operator==(const plane&) const;
    bool operator!=(const plane&) const;

public:
    float dot(const vec4& v) const { return planedot(this, &v); }
    float dotcoord(const vec3& v) const { return planedotcoord(this, &v); }
    float dotnormal(const vec3& v) const { return planedotnormal(this, &v); }
    plane& scale(float s) { return *planescale(this, this, s); }
    plane& scale(const plane& p, float s) { return *planescale(this, &p, s); }
    plane& frompointnormal(const vec3& p, const vec3& normal) { return *planefrompointnormal(this, &p, &normal); }
    plane& normalize() { return *planenormalize(this, this); }
    plane& normalize(const plane& p) { return *planenormalize(this, &p); }
    vec3& intersectline(vec3& v, const vec3& v1, const vec3& v2) const { return *planeintersectline(&v, this, &v1, &v2); }
    plane& frompoints(const vec3& v1, const vec3& v2, const vec3& v3) { return *planefrompoints(this, &v1, &v2, &v3); }
    plane& transform(const matrix& m) { return *planetransform(this, this, &m); }
    plane& transform(const plane& p, const matrix& m) { return *planetransform(this, &p, &m); }
};

__gslib_end__

#include <gslib/math.inl>

#endif

max_stars_repo_name: lymastee/gslib

max_issues_repo_name: lymastee/gslib

max_forks_repo_name: lymastee/gslib


text:
#pragma once
#include "ui/detail/nki_image.h"
#include "ui/detail/nk_essential.h"
#include <gsl/gsl>
#include <nuklear.h>

namespace cws80 {

class UIController;
struct FontRequest;

enum class GraphicsType {
    Other,
    OpenGL,
    Cairo,
    Gdiplus,
};

//
class GraphicsDevice {
public:
    explicit GraphicsDevice(UIController &ctl)
        : ctl_(ctl)
    {
    }
    virtual ~GraphicsDevice() {}

    virtual GraphicsType type() const = 0;

    virtual void setup_context() {}
    virtual void initialize(gsl::span<const FontRequest> fontreqs) = 0;
    virtual void cleanup() = 0;

    im_texture load_texture(const im_image &img);
    virtual im_texture load_texture(const u8 *data, uint w, uint h, uint channels) = 0;
    virtual void unload_texture(nk_handle handle) = 0;

    virtual void render(void *draw_context) = 0;

    virtual nk_user_font *get_font(uint id) = 0;

protected:
    UIController &ctl_;
};

//------------------------------------------------------------------------------
struct FontRequest {
    static FontRequest Default(f32 height, const nk_rune *range);
    static FontRequest File(f32 height, const char *path, const nk_rune *range);
    static FontRequest Memory(f32 height, const void *data, size_t size, const nk_rune *range);

    enum class Type { Default, File, Memory } type;
    f32 height;
    const nk_rune *range;

    union {
        struct {
            const char *path;
        } file;
        struct {
            const void *data;
            size_t size;
        } memory;
    } un;
};

inline auto FontRequest::Default(f32 height, const nk_rune *range) -> FontRequest
{
    FontRequest req;
    req.type = FontRequest::Type::Default;
    req.height = height;
    req.range = range;
    return req;
}

inline auto FontRequest::File(f32 height, const char *path, const nk_rune *range) -> FontRequest
{
    FontRequest req;
    req.type = FontRequest::Type::File;
    req.height = height;
    req.range = range;
    req.un.file.path = path;
    return req;
}

inline auto FontRequest::Memory(f32 height, const void *data, size_t size, const nk_rune *range) -> FontRequest
{
    FontRequest req;
    req.type = FontRequest::Type::Memory;
    req.height = height;
    req.range = range;
    req.un.memory.data = data;
    req.un.memory.size = size;
    return req;
}

}  // namespace cws80

ext: h


text:
#include <math.h>
#include <string.h>

#include <gsl/gsl_matrix.h>

#include <gpc/gpc.h>
#include <egsl/egsl_macros.h>

#include "../csm_all.h"

#include "icp.h"

int icp_loop(struct sm_params*params, const double*q0, double*x_new,
	double*total_error, int*valid, int*iterations) {
	if(any_nan(q0,3)) {
		sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
		return 0;
	}

	LDP laser_sens = params->laser_sens;
	double x_old[3], delta[3], delta_old[3] = {0,0,0};
	copy_d(q0, 3, x_old);
	unsigned int hashes[params->max_iterations];
	int iteration;

	sm_debug("icp: starting at  q0 =  %s  \n", friendly_pose(x_old));

	if(JJ) jj_loop_enter("iterations");

	int all_is_okay = 1;

	for(iteration=0; iteration<params->max_iterations;iteration++) {
		if(JJ) jj_loop_iteration();
		if(JJ) jj_add_double_array("x_old", x_old, 3);

		egsl_push_named("icp_loop iteration");
		sm_debug("== icp_loop: starting iteration. %d  \n", iteration);

		/** Compute laser_sens's points in laser_ref's coordinates
		    by roto-translating by x_old */
		ld_compute_world_coords(laser_sens, x_old);

		/** Find correspondences (the naif or smart way) */
		if(params->use_corr_tricks)
			find_correspondences_tricks(params);
		else
			find_correspondences(params);

		/** If debug_verify_tricks, make sure that find_correspondences_tricks()
		    and find_correspondences() return the same answer */
			if(params->debug_verify_tricks)
				debug_correspondences(params);

		/* If not many correspondences, bail out */
		int num_corr = ld_num_valid_correspondences(laser_sens);
		double fail_perc = 0.05;
		if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
			sm_error("	: before trimming, only %d correspondences.\n",num_corr);
			all_is_okay = 0;
			egsl_pop_named("icp_loop iteration"); /* loop context */
			break;
		}

		if(JJ) jj_add("corr0", corr_to_json(laser_sens->corr, laser_sens->nrays));

		/* Kill some correspondences (using dubious algorithm) */
		if(params->outliers_remove_doubles)
			kill_outliers_double(params);

		int num_corr2 = ld_num_valid_correspondences(laser_sens);

		if(JJ) jj_add("corr1", corr_to_json(laser_sens->corr, laser_sens->nrays));

		double error=0;
		/* Trim correspondences */
		kill_outliers_trim(params, &error);
		int num_corr_after = ld_num_valid_correspondences(laser_sens);

		if(JJ) {
			jj_add("corr2", corr_to_json(laser_sens->corr, laser_sens->nrays));
			jj_add_int("num_corr0", num_corr);
			jj_add_int("num_corr1", num_corr2);
			jj_add_int("num_corr2", num_corr_after);
		}

		*total_error = error;
		*valid = num_corr_after;

		sm_debug("  icp_loop: total error: %f  valid %d   mean = %f\n", *total_error, *valid, *total_error/ *valid);

		/* If not many correspondences, bail out */
		if(num_corr_after < fail_perc * laser_sens->nrays){
			sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
			all_is_okay = 0;
			egsl_pop_named("icp_loop iteration"); /* loop context */
			break;
		}

		/* Compute next estimate based on the correspondences */
		if(!compute_next_estimate(params, x_old, x_new)) {
			sm_error("  icp_loop: Cannot compute next estimate.\n");
			all_is_okay = 0;
			egsl_pop_named("icp_loop iteration");
			break;
		}

		pose_diff_d(x_new, x_old, delta);

		{
			sm_debug("  icp_loop: killing. laser_sens has %d/%d rays valid,  %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
			if(JJ) {
				jj_add_double_array("x_new", x_new, 3);
				jj_add_double_array("delta", delta, 3);
			}
		}
		/** Checks for oscillations */
		hashes[iteration] = ld_corr_hash(laser_sens);

		{
			sm_debug("  icp_loop: it. %d  hash=%d nvalid=%d mean error = %f, x_new= %s\n",
				iteration, hashes[iteration], *valid, *total_error/ *valid,
				friendly_pose(x_new));
		}

		/** PLICP terminates in a finite number of steps! */
		if(params->use_point_to_line_distance) {
			int loop_detected = 0; /* TODO: make function */
			int a; for(a=iteration-1;a>=0;a--) {
				if(hashes[a]==hashes[iteration]) {
					sm_debug("icpc: oscillation detected (cycle length = %d)\n", iteration-a);
					loop_detected = 1;
					break;
				}
			}
			if(loop_detected) {
				egsl_pop_named("icp_loop iteration");
				break;
			}
		}

		/* This termination criterium is useless when using
		   the point-to-line-distance; however, we put it here because
		   one can choose to use the point-to-point distance. */
		if(termination_criterion(params, delta)) {
			egsl_pop_named("icp_loop iteration");
			break;
		}

		copy_d(x_new, 3, x_old);
		copy_d(delta, 3, delta_old);

		egsl_pop_named("icp_loop iteration");
	}

	if(JJ) jj_loop_exit();

	*iterations = iteration+1;

	return all_is_okay;
}

int termination_criterion(struct sm_params*params, const double*delta){
	double a = norm_d(delta);
	double b = fabs(delta[2]);
	return (a<params->epsilon_xy) && (b<params->epsilon_theta);
}

int compute_next_estimate(struct sm_params*params,
	const double x_old[3], double x_new[3])
{
	LDP laser_ref  = params->laser_ref;
	LDP laser_sens = params->laser_sens;

	struct gpc_corr c[laser_sens->nrays];

	int i; int k=0;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!laser_sens->valid[i])
			continue;

		if(!ld_valid_corr(laser_sens,i))
			continue;

		int j1 = laser_sens->corr[i].j1;
		int j2 = laser_sens->corr[i].j2;

		c[k].valid = 1;

		if(laser_sens->corr[i].type == corr_pl) {

			c[k].p[0] = laser_sens->points[i].p[0];
			c[k].p[1] = laser_sens->points[i].p[1];
			c[k].q[0] = laser_ref->points[j1].p[0];
			c[k].q[1] = laser_ref->points[j1].p[1];

			/** TODO: here we could use the estimated alpha */
			double diff[2];
			diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
			diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
			double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
			double normal[2];
			normal[0] = +diff[1] * one_on_norm;
			normal[1] = -diff[0] * one_on_norm;

			double cos_alpha = normal[0];
			double sin_alpha = normal[1];

			c[k].C[0][0] = cos_alpha*cos_alpha;
			c[k].C[1][0] =
			c[k].C[0][1] = cos_alpha*sin_alpha;
			c[k].C[1][1] = sin_alpha*sin_alpha;

/*			sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d,  alpha_seg=%f, cos=%f sin=%f \n", k,i,
				rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/

#if 0
			/* Note: it seems that because of numerical errors this matrix might be
			   not semidef positive. */
			double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
			double trace = c[k].C[0][0] + c[k].C[1][1];

			int semidef = (det >= 0) && (trace>0);
			if(!semidef) {
	/*			printf("%d: Adjusting correspondence weights\n",i);*/
				double eps = -det;
				c[k].C[0][0] += 2*sqrt(eps);
				c[k].C[1][1] += 2*sqrt(eps);
			}
#endif
		} else {
			c[k].p[0] = laser_sens->points[i].p[0];
			c[k].p[1] = laser_sens->points[i].p[1];

			projection_on_segment_d(
				laser_ref->points[j1].p,
				laser_ref->points[j2].p,
				laser_sens->points_w[i].p,
				c[k].q);

			/* Identity matrix */
			c[k].C[0][0] = 1;
			c[k].C[1][0] = 0;
			c[k].C[0][1] = 0;
			c[k].C[1][1] = 1;
		}

		double factor = 1;

		/* Scale the correspondence weight by a factor concerning the
		   information in this reading. */
		if(params->use_ml_weights) {
			int have_alpha = 0;
			double alpha = 0;
			if(!is_nan(laser_ref->true_alpha[j1])) {
				alpha = laser_ref->true_alpha[j1];
				have_alpha = 1;
			} else if(laser_ref->alpha_valid[j1]) {
				alpha = laser_ref->alpha[j1];;
				have_alpha = 1;
			} else have_alpha = 0;

			if(have_alpha) {
				double pose_theta = x_old[2];
				/** Incidence of the ray
					Note that alpha is relative to the first scan (not the world)
					and that pose_theta is the angle of the second scan with
					respect to the first, hence it's ok. */
				double beta = alpha - (pose_theta + laser_sens->theta[i]);
				factor = 1 / square(cos(beta));
			} else {
				static int warned_before = 0;
				if(!warned_before) {
					sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]."
					          "Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
					sm_error("Writing laser_ref: \n");
					ld_write_as_json(laser_ref, stderr);
					warned_before = 1;
				}
			}
		}

		/* Weight the points by the sigma in laser_sens */
		if(params->use_sigma_weights) {
			if(!is_nan(laser_sens->readings_sigma[i])) {
				factor *= 1 / square(laser_sens->readings_sigma[i]);
			} else {
				static int warned_before = 0;
				if(!warned_before) {
					sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
					sm_error("Writing laser_sens: \n");
					ld_write_as_json(laser_sens, stderr);
				}
			}
		}

		c[k].C[0][0] *= factor;
		c[k].C[1][0] *= factor;
		c[k].C[0][1] *= factor;
		c[k].C[1][1] *= factor;

		k++;
	}

	/* TODO: use prior for odometry */
	double std = 0.11;
	const double inv_cov_x0[9] =
		{1/(std*std), 0, 0,
		 0, 1/(std*std), 0,
		 0, 0, 0};

	int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
	if(!ok) {
		sm_error("gpc_solve_valid failed\n");
		return 0;
	}

	double old_error = gpc_total_error(c, k, x_old);
	double new_error = gpc_total_error(c, k, x_new);

	sm_debug("\tcompute_next_estimate: old error: %f  x_old= %s \n", old_error, friendly_pose(x_old));
	sm_debug("\tcompute_next_estimate: new error: %f  x_new= %s \n", new_error, friendly_pose(x_new));
	sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);

	double epsilon = 0.000001;
	if(new_error > old_error + epsilon) {
		sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf  new error: %lf  x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
	}

	return 1;
}

ext: c


text:
/******************************************************************************
CosmoLike Configuration Space Covariances for Projected Galaxy 2-Point Statistics
https://github.com/CosmoLike/CosmoCov
by CosmoLike developers
******************************************************************************/

#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

#include <time.h>
#include <stdio.h>

void cov_G_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_g_interp);
void cov_NG_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_ng_interp);
void cov_G_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_g_interp);
void cov_NG_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_ng_interp);
void cov_G_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_g_interp);
void cov_NG_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_ng_interp);
void cov_G_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_g_interp);
void cov_NG_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_ng_interp);
void cov_G_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_g_interp);
void cov_NG_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_ng_interp);
void cov_G_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_g_interp);
void cov_NG_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_ng_interp);

void cov_G_real_fft_bin_template(double *theta, int Ntheta, double **cov_g_interp, config my_config, double *N, int *array, double *task_type);
void cov_NG_real_fft_bin_template(double *theta, int Ntheta, double **cov_ng_interp, config my_config, int *array, double *task_type);

double func_for_cov_G_shear(double l, int *ar);
double func_for_cov_NG_shear(double l1, double l2, int *ar);
double func_for_cov_G_cl(double l, int *ar);
double func_for_cov_NG_cl(double l1, double l2, int *ar);
double func_for_cov_G_cl_gl(double l, int *ar);
double func_for_cov_NG_cl_gl(double l1, double l2, int *ar);
double func_for_cov_G_cl_shear(double l, int *ar);
double func_for_cov_NG_cl_shear(double l1, double l2, int *ar);
double func_for_cov_G_gl(double l, int *ar);
double func_for_cov_NG_gl(double l1, double l2, int *ar);
double func_for_cov_G_gl_shear(double l, int *ar);
double func_for_cov_NG_gl_shear(double l1, double l2, int *ar);

void cov_G_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_g_interp) {

  // pure noise term
  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;

  int i,j;
  for(i=0;i<Ntheta;i++) {N[i] = 0.;}

  if (z1 ==z3 && z2 ==z4 && pm1 == pm2){ //&& pm1 == pm2 required as C+- doesn't have the diagonal shot noise term
    for(i=0;i<Ntheta;i++) {
      N[i] += pow(survey.sigma_e,4.0)/(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*4.*nsource(z1)*nsource(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
    } //(number of galaxy pairs in the survey contributing to annulus of width Dtheta centered at theta1)^-1
  }
  if (z1 ==z4 && z2 ==z3 && pm1 == pm2){ //&& pm1 == pm2 required as C+- doesn't have the diagonal shot noise term
    for(i=0;i<Ntheta;i++) {
      N[i] += pow(survey.sigma_e,4.0)/(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*4.*nsource(z1)*nsource(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
    }
  }
  for(i=0;i<Ntheta;i++) {
    if(N[i]) {N[i]/= w_mask(theta[i]); printf("N[i]: %lg\n", N[i]);}
  }

  // get big cov_G matrix
  config my_config;
  if (pm1 == 1){my_config.l1 = -0.5;}
  else{my_config.l1 = 3.5;}

  if (pm2 == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}

  // my_config.nu1 = 1.01-my_config.l1/2.;
  // my_config.nu2 = 1.01-my_config.l2/2.;

  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {2,2};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  if (pm1 == 1){my_config.l1 = -0.5;}
  else{my_config.l1 = 3.5;}

  if (pm2 == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}

  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {2,2};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);

}

void cov_G_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_g_interp) {

  // pure noise term
  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;

  int i,j;
  for(i=0;i<Ntheta;i++) {N[i] = 0.;}

  if (z1 ==z3 && z2 ==z4){
    for(i=0;i<Ntheta;i++) {
      N[i] += 1./(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*nlens(z1)*nlens(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor); //(number of galaxy pairs in the survey contributing to annulus of width Dtheta centered at theta1)^-1
    }
  }
  if (z1 ==z4 && z2 ==z3){
    for(i=0;i<Ntheta;i++) {
      N[i] += 1./(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*nlens(z1)*nlens(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
    }
  }
  for(i=0;i<Ntheta;i++) {
    if(N[i]) N[i]/= w_mask(theta[i]);
  }

  // get big cov_G matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = -0.5;

  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,0};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = -0.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;
  double task_type[] = {0,0};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_g_interp) {

  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = zl; array[4] = zs;

  int i,j;

  // get big cov_G matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,1};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = zl; array[4] = zs;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,1};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_g_interp) {

  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;

  int i,j;

  // get big cov_G matrix
  config my_config;
  my_config.l1 = -0.5;
  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,2};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = -0.5;
  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,2};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_g_interp) {

  // pure noise term
  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = zl1; array[2] = zs1; array[3] = zl2; array[4] = zs2;

  int i,j;
  for(i=0;i<Ntheta;i++) {N[i] = 0.;}

  if (zl1 ==zl2 && zs1 ==zs2){
    for(i=0;i<Ntheta;i++) {
      N[i] += pow(survey.sigma_e,2.0)/(2.0*M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*nlens(zl1)*nsource(zs2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
      if(N[i]) N[i]/= w_mask(theta[i]);
    }
  }

  // get big cov_G matrix
  config my_config;
  my_config.l1 = 1.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,1};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_ng_interp) {
  int array[5];
  array[1] = zl1; array[2] = zs1; array[3] = zl2; array[4] = zs2;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = 1.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,1};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_g_interp) {

  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = zl; array[2] = zs; array[3] = z3; array[4] = z4;

  int i,j;

  // get big cov_G matrix
  config my_config;
  my_config.l1 = 1.5;

  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,2};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_ng_interp) {
  int array[5];
  array[1] = zl; array[2] = zs; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = 1.5;

  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,2};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

/*********** Templates *************************************/

void cov_G_real_fft_bin_template(double *theta, int Ntheta, double **cov_g_interp, config my_config, double *N, int *array, double *task_type) {

  my_config.sym_Flag = 0;
  my_config.c_window_width = 0.25;

  double res = 0.;
  int i,j;
  // get input func log-sampled in ell
  const int Nsub = 20; // split each (theta[i],theta[i+1]) range into Nsub sub-ranges, i.e. fill in (Nsub-1) sampling points within the range

  // clock_t start_t, end_t;
  // start_t = clock();

  double ellmax = 50000.;
  double ell_first = 1./theta[Ntheta]; // include the last bin edge
  double ell_last = 1./theta[0];
  double dlnell = log(theta[1]/theta[0])/Nsub;
  int ind_first = floor(log(ell_first)/dlnell); // index of ell_first in log-sampled array (from range [1,ellmax]) containing our orginal desired array (1/theta)
  int Nell = floor(log(ellmax/ell_first)/dlnell) + ind_first + 1; // total sampling points in range [1,ellmax]
  double ell[Nell], func[Nell];
  // printf("ell_first:%lg\n", ell_first);
  // printf("Nell:%d\n", Nell);

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  double noise_factor = 0.;
  double (*func_for_cov_G)(double, int*);
  if(task_type[0]==0) {
    if(task_type[1]==0) {func_for_cov_G = &func_for_cov_G_cl; noise_factor=1.;}
    else if(task_type[1]==1) {func_for_cov_G = &func_for_cov_G_cl_gl;}
    else if(task_type[1]==2) {func_for_cov_G = &func_for_cov_G_cl_shear;}
  }
  else if(task_type[0]==1) {
    if(task_type[1]==1) {func_for_cov_G = &func_for_cov_G_gl; noise_factor=1.;}
    else if(task_type[1]==2) {func_for_cov_G = &func_for_cov_G_gl_shear;}
  }
  else {
    if(task_type[1]==2) {func_for_cov_G = &func_for_cov_G_shear; noise_factor=2.;}
  }

  for(i=0; i<Nell; i++) {
    ell[i] = exp(dlnell * (i-ind_first)) * ell_first;
    func[i] = func_for_cov_G(ell[i], array);
  }

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  // extrapolate
  // const int N_extra = 800-246;
  const int N_extra = 800;
  int Ntot = N_extra*2+Nell;
  double large_ell[Ntot], large_fl[Ntot];
  extrap_log_linear_cfastcov(ell, Nell, N_extra, large_ell);
  extrap_log_linear_cfastcov(func, Nell, N_extra, large_fl);

  double **fk1k2;
  fk1k2 = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    fk1k2[i] = malloc(Ntot*sizeof(double));
  }
  mk_diag_g_to_ng(large_fl, Ntot, dlnell, fk1k2);

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  // do integration
  double theta_fine[Ntot];
  double **result;
  result = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    result[i] = malloc(Ntot*sizeof(double));
  }
  // twobessel(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, theta_fine, theta_fine, result);
  double smooth_dlnr = dlnell*Nsub;
  printf("Ntot,%d\n",Ntot);
  twobessel_binave(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, smooth_dlnr, 2.5, theta_fine, theta_fine, result);
  // printf("Ntot,%d\n",Ntot);
  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  int ind_theta_first = Ntot - 1 - N_extra-ind_first - Nsub*(Ntheta); // different from nobin case because ind_first is set different!
  for(i=0; i<Ntheta; i++) {
    for(j=0; j<Ntheta; j++) {
      cov_g_interp[i][j] = result[ind_theta_first+i*Nsub][ind_theta_first+j*Nsub];
      cov_g_interp[i][j] /= ( (theta[i+1]*theta[i+1]-theta[i]*theta[i])*(theta[j+1]*theta[j+1]-theta[j]*theta[j]) *M_PI*M_PI*M_PI*survey.area/41253.);
    }
    // printf("theta[%d]=%lg, cov_g_interp[%d][%d]: %lg\n",i, theta[i], i,i, cov_g_interp[i][i]);
    cov_g_interp[i][i] += noise_factor * N[i];
  }

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  free(result);free(fk1k2);

}

void cov_NG_real_fft_bin_template(double *theta, int Ntheta, double **cov_ng_interp, config my_config, int *array, double *task_type) {
  int i,j;
  my_config.sym_Flag = 0;
  my_config.c_window_width = 0.25;

  // get input func log-sampled in ell
  const int Nsub = 20; // split each (theta[i],theta[i+1]) range into Nsub sub-ranges, i.e. fill in (Nsub-1) sampling points within the range

  // clock_t start_t, end_t;
  // start_t = clock();

  double ellmax = 50000.;
  double ell_first = 1./theta[Ntheta]; // include the last bin edge
  double ell_last = 1./theta[0];
  double dlnell = log(theta[1]/theta[0])/Nsub;
  int ind_first = floor(log(ell_first)/dlnell); // index of ell_first in log-sampled array (from range [1,ellmax]) containing our orginal desired array (1/theta)
  int Nell = floor(log(ellmax/ell_first)/dlnell) + ind_first + 1; // total sampling points in range [1,ellmax]
  double ell[Nell];
  // printf("ell_first:%lg\n", ell_first);
  // printf("Nell:%d\n", Nell);
  for(i=0; i<Nell; i++) {
    ell[i] = exp(dlnell * (i-ind_first)) * ell_first;
  }

  double (*func_for_cov_NG)(double, double, int*);
  if(task_type[0]==0) {
    if(task_type[1]==0) {func_for_cov_NG = &func_for_cov_NG_cl;}
    else if(task_type[1]==1) {func_for_cov_NG = &func_for_cov_NG_cl_gl;}
    else if(task_type[1]==2) {func_for_cov_NG = &func_for_cov_NG_cl_shear;}
  }
  else if(task_type[0]==1) {
    if(task_type[1]==1) {func_for_cov_NG = &func_for_cov_NG_gl;}
    else if(task_type[1]==2) {func_for_cov_NG = &func_for_cov_NG_gl_shear;}
  }
  else {
    if(task_type[1]==2) {func_for_cov_NG = &func_for_cov_NG_shear;}
  }

  double **func;
  func = malloc(Nell*sizeof(double *));
  for(i=0; i<Nell; i++) {
    func[i] = malloc(Nell * sizeof(double));
    for(j=0; j<Nell; j++) {
      func[i][j] = func_for_cov_NG(ell[i], ell[j], array);
    }
  }

  // FILE * output = fopen("func_ng.txt", "w");
  // for(i=0;i<Nell;i++){
  //   for(j=0;j<Nell;j++){
  //   fprintf(output, "%lg ", func[i][j]);
  // }
  // fprintf(output, "\n");
  // }
  // fclose(output);
  // exit(0);

  // extrapolate
  const int N_extra = 800;
  int Ntot = N_extra*2+Nell;
  double large_ell[Ntot];
  double **fk1k2;
  fk1k2 = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    fk1k2[i] = malloc(Ntot*sizeof(double));
  }

  extrap_log_linear_cfastcov(ell, Nell, N_extra, large_ell);
  extrap_2dzeros(func, Nell, N_extra, fk1k2);

  // do integration
  double theta_fine[Ntot];
  double **result;
  result = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    result[i] = malloc(Ntot*sizeof(double));
  }
  // twobessel(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, theta_fine, theta_fine, result);
  double smooth_dlnr = dlnell*Nsub;
  twobessel_binave(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, smooth_dlnr, 2.5, theta_fine, theta_fine, result);
  printf("Ntot,%d\n",Ntot);

  int ind_theta_first = Ntot - 1 - N_extra-ind_first - Nsub*(Ntheta); // different from nobin case because ind_first is set different!
  for(i=0; i<Ntheta; i++) {
    for(j=0; j<Ntheta; j++) {
      cov_ng_interp[i][j] = result[ind_theta_first+i*Nsub][ind_theta_first+j*Nsub];
      cov_ng_interp[i][j] /= ( (theta[i+1]*theta[i+1]-theta[i]*theta[i])*(theta[j+1]*theta[j+1]-theta[j]*theta[j]) /2. *M_PI*M_PI*M_PI);
    }
  }

  free(result);free(fk1k2);free(func);
}

/********** Functions for differnt covariances ***************************************/
double func_for_cov_G_shear(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_shear_shear_IA_tab(l,n1,n3);
  C24 = C_shear_shear_IA_tab(l,n2,n4);
  C14 = C_shear_shear_IA_tab(l,n1,n4);
  C23 = C_shear_shear_IA_tab(l,n2,n3);

  if (n1 == n3){N13= pow(survey.sigma_e,2.0)/(2.*nsource(n1)*survey.n_gal_conversion_factor);}
  if (n1 == n4){N14= pow(survey.sigma_e,2.0)/(2.*nsource(n1)*survey.n_gal_conversion_factor);}
  if (n2 == n3){N23= pow(survey.sigma_e,2.0)/(2.*nsource(n2)*survey.n_gal_conversion_factor);}
  if (n2 == n4){N24= pow(survey.sigma_e,2.0)/(2.*nsource(n2)*survey.n_gal_conversion_factor);}

  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_shear(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_shear_shear_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);

}

double func_for_cov_G_cl(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_cl_tomo(l,n1,n3);
  C24 = C_cl_tomo(l,n2,n4);
  C14 = C_cl_tomo(l,n1,n4);
  C23 = C_cl_tomo(l,n2,n3);

  N13= tomo.n_lens_ij[n1][n3]/(nlens(n1)*nlens(n3)*survey.n_gal_conversion_factor);
  N14= tomo.n_lens_ij[n1][n4]/(nlens(n1)*nlens(n4)*survey.n_gal_conversion_factor);
  N23= tomo.n_lens_ij[n2][n3]/(nlens(n2)*nlens(n3)*survey.n_gal_conversion_factor);
  N24= tomo.n_lens_ij[n2][n4]/(nlens(n2)*nlens(n4)*survey.n_gal_conversion_factor);

  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_cl(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_cl_cl_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_cl_gl(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_cl_tomo(l,n1,n3);
  C24 = C_ggl_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_cl_tomo(l,n2,n3);

  N13= tomo.n_lens_ij[n1][n3]/(nlens(n1)*nlens(n3)*survey.n_gal_conversion_factor);
  N23= tomo.n_lens_ij[n2][n3]/(nlens(n2)*nlens(n3)*survey.n_gal_conversion_factor);
  //printf("%lg, %lg, %lg, %lg, %lg, %lg, %lg\n", l, C13, C24, C14, C23, N13, N23);
  return (C13*C24+C13*N24+C24*N13+C14*C23+C14*N23+C23*N14)*l*l*l;
}

double func_for_cov_NG_cl_gl(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_cl_gl_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_cl_shear(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_ggl_IA_tab(l,n1,n3);
  C24 = C_ggl_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_ggl_IA_tab(l,n2,n3);

  return (C13*C24+ C14*C23)*l*l*l;
}

double func_for_cov_NG_cl_shear(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_cl_shear_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_gl(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_cl_tomo(l,n1,n3);
  C24 = C_shear_shear_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_ggl_IA_tab(l,n3,n2);

  N13= tomo.n_lens_ij[n1][n3]/(nlens(n1)*nlens(n3)*survey.n_gal_conversion_factor);
  if (n2 == n4){N24= pow(survey.sigma_e,2.0)/(2.0*nsource(n2)*survey.n_gal_conversion_factor);}
  // printf("%lg, %lg, %lg, %lg, %lg, %lg, %lg\n", l, C13, C24, C14, C23, N13, N24);
  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_gl(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_gl_gl_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_gl_shear(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_ggl_IA_tab(l,n1,n3);
  C24 = C_shear_shear_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_shear_shear_IA_tab(l,n2,n3);
  if (n2 == n4){N24= pow(survey.sigma_e,2.0)/(2.0*nsource(n2)*survey.n_gal_conversion_factor);}
  if (n2 == n3){N23= pow(survey.sigma_e,2.0)/(2.0*nsource(n2)*survey.n_gal_conversion_factor);}

  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_gl_shear(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_gl_shear_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}


text:
#ifndef _SEARCHBEST_
#define _SEARCHBEST_

#include <assert.h>
#include <cmath>
#include <float.h>
#include <climits>
// use openblas
#include <cblas.h>
#include "cosine_similarity.h"

// Step 1, g++ main.cpp search_best.cpp cosine_similarity.cpp -std=c++11
// Step 2, g++ main.cpp search_best.cpp cosine_similarity.cpp -std=c++11 -O3
// Step 3, g++ main.cpp search_best.cpp cosine_similarity.cpp -std=c++11 -O3 -Ofast -ffast-math
template <typename T>
int SearchBest(const T* __restrict__ const pVecA,  // 待搜索的单个特征向量首地址
               const int lenA,  // 待搜索特征向量长度(1 x 单个特征维数)
               const T* __restrict__ const pVecDB, // 底库首地址
               const int lenDB) // 底库长度(特征个数 x 单个特征维数)
{
    assert(lenDB%lenA == 0);
    const int featsize = lenA;
    const int facenum  = lenDB / lenA;

    int best_index = - INT_MAX;
    T best_similarity = - FLT_MAX;
#if 0
// Step 5, 加上OpenMP
//GCC很聪明，OpenMP默认线程数就是多核处理器的核心数量，不必显示指定
//OpenMP起线程，收回线程也是有开销的，所以要合理安排每个线程的任务量大小，不宜放入内层for循环（任务量太小划不来）
//#pragma omp parallel for num_threads(8)
#pragma omp parallel for
    for(int i = 0; i < facenum; i++) {
        // 普通C++代码实现的余弦相似度计算
        T similarity = Cosine_similarity(pVecA, pVecDB + i*featsize, featsize);
        // 使用向量化代码实现的余弦相似度计算
        //T similarity = Cosine_similarity_avx(pVecA, pVecDB + i*featsize, featsize);
        if(similarity > best_similarity) {
            best_similarity = similarity;
            best_index = i;
        }
    }

#else
// Step 12，使用OpenBLAS
    T simAll[facenum] = {0.0f};
    cblas_sgemv(CblasRowMajor, CblasNoTrans, facenum, featsize, 1, pVecDB, featsize, pVecA, 1, 0, simAll, 1);
    // 寻找simAll里面最大的，它的序号就是要找的id
    for(int i = 0; i < facenum; i++) {
        if(simAll[i] > best_similarity) {
            best_similarity = simAll[i];
            best_index = i;
        }
    }
#endif

    return best_index;
}

#endif //!_SEARCHBEST_

max_stars_repo_name: awesome-archive/CaptainBlackboard


text:
#include "linearwaves.h"
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

#define FREAD_CHECK(i,j) ((i) >= (j)) ? (void) 0 : printf("fread returned read fewer items than expected (%zu < %zu) at line %d in %s of %s\n",i,j,__LINE__,__func__,__FILE__)

void interpolation(double *xd, double *yd, int nd, double *x, double *y, double *dy, double *d2y, int n) {
    gsl_interp_accel *acc = gsl_interp_accel_alloc ();
    gsl_spline *spline = gsl_spline_alloc (gsl_interp_cspline, nd);

    gsl_spline_init (spline, xd, yd, nd);

    int i;
    for(i=0;i<n;i++) {
        y[i] = gsl_spline_eval(spline,x[i],acc);
        dy[i] = gsl_spline_eval_deriv(spline,x[i],acc);
        d2y[i] = gsl_spline_eval_deriv2(spline,x[i],acc);
    }
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);

    return;
}

void interpolate_onto_grid(double *xd, double *yd, int ndata, double *lr, double *ynew, double *dlydlr, double *d2lydlr, int n,int add_flag) {
    int i;
    int jstart = 0;
    int jend = n-1;

    for(i=0;i<n;i++) {
        if (lr[i] >= xd[0]) {
            jstart = i;
            break;
        }
    }
    for(i=jstart;i<n;i++) {
        if (lr[i] >= xd[ndata-1]) {
            if (lr[i] == xd[ndata-1]) {
                jend = i;
            }
            else {
                jend = i-1;
            }
            break;
        }
    }
    int n_interp = jend-jstart +1;

    interpolation(xd,yd,ndata,&lr[jstart],&ynew[jstart],&dlydlr[jstart],&d2lydlr[jstart],n_interp);

    for(i=0;i<jstart;i++) {
        ynew[i] = ynew[jstart] + dlydlr[jstart]*(lr[i]-lr[jstart]);
        dlydlr[i] = dlydlr[jstart];
        d2lydlr[i] = 0;
    }
    for(i=jend+1;i<n;i++) {
        ynew[i] = ynew[jend] + dlydlr[jend]*(lr[i]-lr[jend]);
        dlydlr[i] = dlydlr[jend];
        d2lydlr[i] = 0;
    }

// Add in inner truncation and outer truncation
    if (add_flag) {
        double ri = exp(lr[0])*.99;
        double ro = exp(lr[n-1])/2.0;
        double x;
        for(i=0;i<n;i++) {
            x = exp(lr[i]);
            ynew[i] += log(1 - sqrt(ri/x)) - pow(x/ro,2);
            dlydlr[i] += .5*sqrt(ri/x)*pow(1-sqrt(ri/x),-1) - 2*pow(x/ro,2);
            d2lydlr[i] += -.25*sqrt(ri/x)*pow(1-sqrt(ri/x),-2) - 4*pow(x/ro,2);
        }
    }
    return;

}

void read_sigma(char *fname, double *lr, double *sigma, double *dlsdlr, double *d2lsdlr, double *omega, double *dlomdlr, double *d2lomdlr, int n,int readomega) {
    double temp;
    size_t ndata;
    double *xd,*yd;
    int i;
    size_t fres;

    FILE *f = fopen(fname,"r");
    ndata = 1;
    fres = fread(&temp,sizeof(double),ndata,f);
    FREAD_CHECK(fres,ndata);
    ndata=(size_t)temp;

    xd = (double *)malloc(sizeof(double)*ndata);
    yd = (double *)malloc(sizeof(double)*ndata);

    fres = fread(xd,sizeof(double),ndata,f);
    FREAD_CHECK(fres,ndata);
    fres =  fread(yd,sizeof(double),ndata,f);
    FREAD_CHECK(fres,ndata);

    interpolate_onto_grid(xd,yd,ndata,lr,sigma,dlsdlr,d2lsdlr,n,TRUE);

    if (readomega) {
        fres =  fread(yd,sizeof(double),ndata,f);
        FREAD_CHECK(fres,ndata);
        for(i=0;i<(int)ndata;i++) {
            yd[i] = log( exp(yd[i])/pow(exp(xd[i]),2));
        }
        interpolate_onto_grid(xd,yd,ndata,lr,omega,dlomdlr,d2lomdlr,n,FALSE);
    }

    fclose(f);
    SAFE_FREE(xd);
    SAFE_FREE(yd);
    return;
}
/*
int main(void) {
    double *x,*y,*dy,*d2y;
    int i;

    double rmin = .01;
    double rmax = 10;
    int n = 1000;
    x = (double *)malloc(sizeof(double)*n);
    y = (double *)malloc(sizeof(double)*n);
    dy = (double *)malloc(sizeof(double)*n);
    d2y = (double *)malloc(sizeof(double)*n);

    for(i=0;i<n;i++) {
        x[i] = log(rmin) + i*log(rmax/rmin)/n;
        y[i]  = 0;
        dy[i] = 0;
        d2y[i] = 0;
    }

    read_sigma("../profiles/avg_profs/prof_q1e-3_a1e-2.bin",x,y,dy,d2y,n);

    FILE *f = fopen("interp_res.dat","w");

    double temp = (double)n;
    fwrite(&temp,sizeof(double),1,f);
    fwrite(x,sizeof(double),n,f);
    fwrite(y,sizeof(double),n,f);
    fwrite(dy,sizeof(double),n,f);
    fwrite(d2y,sizeof(double),n,f);
    SAFE_FREE(x);
    SAFE_FREE(y);
    SAFE_FREE(dy);
    SAFE_FREE(d2y);
}
*/


text:
#include <stdio.h>
#include <math.h>

#include <gsl/gsl_integration.h>
#include "CosmoCalcs.h"

int main() {
	UniverseLCDM myU;
	double err = 0.0;
	double x, y, z;

	double Wl = 0.75;
	double Wm = 0.05;
	double Wr = 1e-6;
	double Wk = 1.0 - Wl - Wm - Wr;

	InitUniverse( &myU, 70.0 * MpcPerMeter * 1e3, Wl, Wm, Wr, false, true );

	/*x = Dc(0.5, &err);
	printf("GSL value, unc: %.16e, %.16e\n", x, err);*/

	printf("Wl = %.16e, Wm = %.16e, Wr = %.16e\n", myU.OmegaLambda, myU.OmegaMatter, myU.OmegaRelativistic );

	//z = 0.484375;
	z = 0.5048828125;
	y = tL0_Cosmic(&myU, z, true, 1e-6, 1e-6, 128, &err);
	printf("z = %f, tL0 = %.16e, z' = %.16e\n", z, y, tL0Inv_Cosmic(&myU, y, 0, 1e-9, 1024));

	z = 0.5;
	y = tL0_Cosmic(&myU, z, true, 1e-6, 1e-6, 128, &err);
	printf("z = %f, tL0 = %.16e, z' = %.16e\n", z, y, tL0Inv_Cosmic(&myU, y, 0, 1e-9, 1024));

	z = 0.75;
	y = tL0_Cosmic(&myU, z, true, 1e-6, 1e-6, 128, &err);
	printf("z = %f, tL0 = %.16e, z' = %.16e\n", z, y, tL0Inv_Cosmic(&myU, y, 0, 1e-9, 1024));

	z = 1.0;
	y = tL0_Cosmic(&myU, z, true, 1e-6, 1e-6, 128, &err);
	printf("z = %f, tL0 = %.16e, z' = %.16e\n", z, y, tL0Inv_Cosmic(&myU, y, 0, 1e-9, 1024));

	z = 5.0;
	y = tL0_Cosmic(&myU, z, true, 1e-6, 1e-6, 128, &err);
	printf("z = %f, tL0 = %.16e, z' = %.16e\n", z, y, tL0Inv_Cosmic(&myU, y, 2, 1e-9, 1024));

	return 0;
}


text:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <nlopt.h>

// System definition and dimension

int mysys = 0;
int dim = 3;

// Semi-empirical method and parameter variation range

char * method = "pm7";
double pdev = 0.7;

// Algorithm parameters

nlopt_algorithm global_alg = NLOPT_GD_MLSL_LDS;
nlopt_algorithm local_alg = NLOPT_LN_BOBYQA;
int maxeval = 3;
double minrms = 0.01;
double tol = 0.001;

//Define here  your system's input for MOPAC

void gen_srpgeo(int ndat, double ** data) {
	long length;
	char * buffer = 0;
	char procedure[0x100];

	switch (mysys) {
	case 0: {
		// Ar/C10H8
		FILE * fp = fopen("./naf_geo.xyz", "r");
		if (!fp)
			exit(EXIT_FAILURE);
		fseek(fp, 0L, SEEK_END);
		length = ftell(fp);
		rewind(fp);
		buffer = (char *) malloc((length+1) * sizeof(char));
		if (buffer) {
			fread(buffer, sizeof(char), length, fp);
		}
		fclose(fp);

		for (int i = 0; i < ndat; ++i) {
			char buf[0x100];
			snprintf(buf, sizeof(buf), "./inp_semp/geo_%d.mop", i);
			FILE * fq = fopen(buf, "w");
			strcpy(procedure, method);
			strcpy(procedure, " charge=0 1scf EXTERNAL=mopac_parameter\n");
			fprintf(fq, "%s", procedure);
			fprintf(fq, "Dumb title rule\n");
			fprintf(fq, " \n");
			fprintf(fq, "Ar %f %f %f\n", data[i][0], data[i][1], data[i][2]);
			fputs(buffer, fq);
			fprintf(fq, " ");
			fclose(fq);
		}
		free(buffer);
	}

	default:
		break;
	}
}


text:
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_blas.h>

int main() {
	int ret;
	int i, j;
	gsl_vector* tau;
	gsl_matrix *A;
	gsl_matrix *Q, *R, *RTR;
	gsl_matrix_view Rtop;
	int M = 4;
	int N = 3;

	/*
	  gsl_matrix A;
	  double data[9];
	  memset(&A, 0, sizeof(gsl_matrix));
	  A.size1 = 3;
	  A.size2 = 3;
	  A.tda = 3;
	  A.data = data;
	  gsl_matrix_set(&A, 0, 0, 34.0);
	  gsl_matrix_set(&A, 0, 1, 4.0);
	  gsl_matrix_set(&A, 0, 2, 14.0);
	  gsl_matrix_set(&A, 1, 0, 1.0);
	  gsl_matrix_set(&A, 1, 1, 8.0);
	  gsl_matrix_set(&A, 1, 2, 3.0);
	  gsl_matrix_set(&A, 2, 0, 7.0);
	  gsl_matrix_set(&A, 2, 1, 1.0);
	  gsl_matrix_set(&A, 2, 2, 8.0);
	*/

	A = gsl_matrix_alloc(M, N);

	for (i=0; i<M; i++)
		for (j=0; j<N; j++)
			gsl_matrix_set(A, i, j, (double)rand()/(double)RAND_MAX);

	for (i=0; i<A->size1; i++) {
		printf((i==0) ? "A = (" : "    (");
		for (j=0; j<A->size2; j++) {
			printf(" %12.5g ", gsl_matrix_get(A, i, j));
		}
		printf(")\n");
	}
	printf("\n");

	tau = gsl_vector_alloc(N);

	ret = gsl_linalg_QR_decomp(A, tau);

	Q = gsl_matrix_alloc(M, M);
	R = gsl_matrix_alloc(M, N);

	ret = gsl_linalg_QR_unpack(A, tau, Q, R);

	for (i=0; i<Q->size1; i++) {
		printf((i==0) ? "Q = (" : "    (");
		for (j=0; j<Q->size2; j++) {
			printf(" %12.5g ", gsl_matrix_get(Q, i, j));
		}
		printf(")\n");
	}
	printf("\n");

	for (i=0; i<R->size1; i++) {
		printf((i==0) ? "R = (" : "    (");
		for (j=0; j<R->size2; j++) {
			printf(" %12.5g ", gsl_matrix_get(R, i, j));
		}
		printf(")\n");
	}
	printf("\n");

	Rtop = gsl_matrix_submatrix(R, 0, 0, N, N);
	RTR = gsl_matrix_alloc(N, N);
	gsl_matrix_memcpy(RTR, &(Rtop.matrix));
	ret = gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit,
						 1.0, RTR, RTR);
	//(Rtop.matrix), &(Rtop.matrix));

	for (i=0; i<RTR->size1; i++) {
		printf((i==0) ? "RTR = (" : "      (");
		for (j=0; j<RTR->size2; j++) {
			printf(" %12.5g ", gsl_matrix_get(RTR, i, j));
		}
		printf(")\n");
	}
	printf("\n");

	gsl_matrix_free(RTR);

	gsl_matrix_free(Q);
	gsl_matrix_free(R);
	gsl_vector_free(tau);

	gsl_matrix_free(A);

	return 0;
}

max_stars_repo_licenses: Net-SNMP Xnet

max_issues_repo_licenses: Net-SNMP Xnet

max_forks_repo_licenses: Net-SNMP Xnet


text:
#include <petsc.h>
#include "plexview.h"

PetscErrorCode PlexViewFromOptions(DM plex) {
    PetscErrorCode ierr;
    PetscBool  cell_cones = PETSC_FALSE,
               closures_coords = PETSC_FALSE,
               coords = PETSC_FALSE,
               points = PETSC_FALSE,
               use_height = PETSC_FALSE,
               vertex_supports = PETSC_FALSE;

    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "plex_view_", "view options for tiny", "");CHKERRQ(ierr);
    ierr = PetscOptionsBool("-cell_cones", "print cones of each cell",
                            "tiny.c", cell_cones, &cell_cones, NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-closures_coords", "print vertex and edge (centers) coordinates for each cell",
                            "tiny.c", closures_coords, &closures_coords, NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-coords", "print section and local vec for vertex coordinates",
                            "tiny.c", coords, &coords, NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-points", "print point index ranges for vertices,edges,cells",
                            "tiny.c", points, &points, NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-use_height", "use Height instead of Depth when printing points",
                            "tiny.c", use_height, &use_height, NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-vertex_supports", "print supports of each vertex",
                            "tiny.c", vertex_supports, &vertex_supports, NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnd();

    if (points) {
        ierr = PlexViewPointRanges(plex,use_height); CHKERRQ(ierr);
    }
    if (cell_cones) {
        ierr = PlexViewFans(plex,2,2,1); CHKERRQ(ierr);
    }
    if (vertex_supports) {
        ierr = PlexViewFans(plex,2,0,1); CHKERRQ(ierr);
    }
    if (coords) {
        ierr = PlexViewCoords(plex); CHKERRQ(ierr);
    }
    if (closures_coords) {
        ierr = PlexViewClosuresCoords(plex); CHKERRQ(ierr);
    }
    return 0;
}

static const char* stratanames[4][10] =
                       {{"vertex","",    "",    ""},       // dim=0 names
                        {"vertex","cell","",    ""},       // dim=1 names
                        {"vertex","edge","cell",""},       // dim=2 names
                        {"vertex","edge","face","cell"}};  // dim=3 names

PetscErrorCode PlexViewPointRanges(DM plex, PetscBool use_height) {
    PetscErrorCode ierr;
    int         dim, m, start, end;
    const char  *plexname;
    MPI_Comm    comm;
    PetscMPIInt rank,size;

    ierr = PetscObjectGetComm((PetscObject)plex,&comm); CHKERRQ(ierr);
    ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
    ierr = MPI_Comm_rank(comm,&rank); CHKERRQ(ierr);
    ierr = DMGetDimension(plex,&dim); CHKERRQ(ierr);
    ierr = PetscObjectGetName((PetscObject)plex,&plexname); CHKERRQ(ierr);
    ierr = PetscPrintf(comm,"point ranges for DMPlex %s in %dD:\n",plexname,dim); CHKERRQ(ierr);
    if (size > 1) {
        ierr = PetscSynchronizedPrintf(comm,"  [rank %d]",rank); CHKERRQ(ierr);
    }
    ierr = DMPlexGetChart(plex,&start,&end); CHKERRQ(ierr);
    if (end < 1) { // nothing on this rank
        ierr = PetscSynchronizedPrintf(comm,"\n"); CHKERRQ(ierr);
        ierr = PetscSynchronizedFlush(comm,PETSC_STDOUT); CHKERRQ(ierr);
        return 0;
    }
    ierr = PetscSynchronizedPrintf(comm,
        "  chart points %d,...,%d\n",start,end-1); CHKERRQ(ierr);
    for (m = 0; m < dim + 1; m++) {
        if (use_height) {
            ierr = DMPlexGetHeightStratum(plex,m,&start,&end); CHKERRQ(ierr);
            ierr = PetscSynchronizedPrintf(comm,
                "    height %d of size %d: %d,...,%d (%s)\n",
                m,end-start,start,end-1,dim < 4 ? stratanames[dim][2-m] : ""); CHKERRQ(ierr);
        } else {
            ierr = DMPlexGetDepthStratum(plex,m,&start,&end); CHKERRQ(ierr);
            ierr = PetscSynchronizedPrintf(comm,
                "    depth=dim %d of size %d: %d,...,%d (%s)\n",
                m,end-start,start,end-1,dim < 4 ? stratanames[dim][m] : ""); CHKERRQ(ierr);
        }
    }
    ierr = PetscSynchronizedFlush(comm,PETSC_STDOUT); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode PlexViewFans(DM plex, int dim, int basestrata, int targetstrata) {
    PetscErrorCode ierr;
    const char  *plexname;
    const int   *targets;
    int         j, m, start, end, cssize;
    MPI_Comm    comm;
    PetscMPIInt rank,size;

    ierr = PetscObjectGetComm((PetscObject)plex,&comm); CHKERRQ(ierr);
    ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
    ierr = MPI_Comm_rank(comm,&rank); CHKERRQ(ierr);
    ierr = PetscObjectGetName((PetscObject)plex,&plexname); CHKERRQ(ierr);
    ierr = PetscPrintf(comm,"fans (cones or supports) for DMPlex %s:\n",plexname); CHKERRQ(ierr);
    if (size > 1) {
        ierr = PetscSynchronizedPrintf(comm,"  [rank %d]",rank); CHKERRQ(ierr);
    }
    ierr = PetscSynchronizedPrintf(comm,
        "  %s (= %s indices) of each %s\n",
        (basestrata > targetstrata) ? "cones" : "supports",
        stratanames[dim][targetstrata],stratanames[dim][basestrata]); CHKERRQ(ierr);
    ierr = DMPlexGetDepthStratum(plex, basestrata, &start, &end); CHKERRQ(ierr);
    for (m = start; m < end; m++) {
        if (basestrata > targetstrata) {
            ierr = DMPlexGetConeSize(plex,m,&cssize); CHKERRQ(ierr);
            ierr = DMPlexGetCone(plex,m,&targets); CHKERRQ(ierr);
        } else {
            ierr = DMPlexGetSupportSize(plex,m,&cssize); CHKERRQ(ierr);
            ierr = DMPlexGetSupport(plex,m,&targets); CHKERRQ(ierr);
        }
        ierr = PetscSynchronizedPrintf(comm,
            "    %s %d: ",stratanames[dim][basestrata],m); CHKERRQ(ierr);
        for (j = 0; j < cssize-1; j++) {
            ierr = PetscSynchronizedPrintf(comm,
                "%d,",targets[j]); CHKERRQ(ierr);
        }
        ierr = PetscSynchronizedPrintf(comm,
            "%d\n",targets[cssize-1]); CHKERRQ(ierr);
    }
    ierr = PetscSynchronizedFlush(comm,PETSC_STDOUT); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode PlexViewCoords(DM plex) {
    PetscErrorCode ierr;
    PetscSection coordSection;
    Vec          coordVec;
    const char   *plexname;

    ierr = PetscObjectGetName((PetscObject)plex,&plexname); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,"coordinate PetscSection and Vec for DMPlex %s:\n",plexname); CHKERRQ(ierr);
    ierr = DMGetCoordinateSection(plex, &coordSection); CHKERRQ(ierr);
    if (coordSection) {
        ierr = PetscSectionView(coordSection,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);
    } else {
        ierr = PetscPrintf(PETSC_COMM_WORLD,
            "[PlexViewCoords():  vertex coordinates PetscSection has not been set]\n"); CHKERRQ(ierr);
    }
    ierr = DMGetCoordinatesLocal(plex,&coordVec); CHKERRQ(ierr);
    if (coordVec) {
        ierr = VecViewLocalStdout(coordVec,PETSC_COMM_WORLD); CHKERRQ(ierr);
    } else {
        ierr = PetscPrintf(PETSC_COMM_WORLD,
            "[PlexViewCoords():  vertex coordinates Vec not been set]\n"); CHKERRQ(ierr);
    }
    return 0;
}

PetscErrorCode PlexViewClosuresCoords(DM plex) {
    PetscErrorCode ierr;
    DM          cdm;
    Vec         coords;
    double      *acoords;
    int         numpts, *pts = NULL,
                j, p, vertexstart, vertexend, edgeend, cellstart, cellend;
    MPI_Comm    comm;
    PetscMPIInt rank,size;
    const char  *plexname;

    ierr = PetscObjectGetComm((PetscObject)plex,&comm); CHKERRQ(ierr);
    ierr = MPI_Comm_size(comm,&size);CHKERRQ(ierr);
    ierr = MPI_Comm_rank(comm,&rank); CHKERRQ(ierr);
    ierr = PetscObjectGetName((PetscObject)plex,&plexname); CHKERRQ(ierr);
    ierr = PetscPrintf(comm,
        "closure points and coordinates for each cell in DMPlex %s:\n",plexname); CHKERRQ(ierr);
    if (size > 1) {
        ierr = PetscSynchronizedPrintf(comm,"  [rank %d]",rank); CHKERRQ(ierr);
    }
    ierr = DMPlexGetHeightStratum(plex, 0, &cellstart, &cellend); CHKERRQ(ierr);
    if (cellend < 1) { // nothing on this rank
        ierr = PetscSynchronizedPrintf(comm,"\n"); CHKERRQ(ierr);
        ierr = PetscSynchronizedFlush(comm,PETSC_STDOUT); CHKERRQ(ierr);
        return 0;
    }
    ierr = DMGetCoordinateDM(plex, &cdm); CHKERRQ(ierr);
    ierr = DMGetCoordinatesLocal(plex,&coords); CHKERRQ(ierr);
    ierr = DMPlexGetDepthStratum(plex, 0, &vertexstart, &vertexend); CHKERRQ(ierr);
    ierr = DMPlexGetDepthStratum(plex, 1, NULL, &edgeend); CHKERRQ(ierr);
    ierr = VecGetArray(coords, &acoords); CHKERRQ(ierr);
    for (j = cellstart; j < cellend; j++) {
        ierr = DMPlexGetTransitiveClosure(plex, j, PETSC_TRUE, &numpts, &pts);
        ierr = PetscSynchronizedPrintf(comm,"  cell %d\n",j); CHKERRQ(ierr);
        for (p = 0; p < numpts*2; p += 2) {   // omit orientations
            if ((pts[p] >= vertexstart) && (pts[p] < edgeend)) { // omit cells in closure
                if (pts[p] < vertexend) { // get location from coords
                    int voff;
                    voff = pts[p] - vertexstart;
                    ierr = PetscSynchronizedPrintf(comm,
                        "    vertex %3d at (%g,%g)\n",
                        pts[p],acoords[2*voff+0],acoords[2*voff+1]); CHKERRQ(ierr);
                } else { // assume it is an edge ... compute center
                    const int *vertices;
                    int       voff[2];
                    double    x,y;
                    ierr = DMPlexGetCone(plex, pts[p], &vertices); CHKERRQ(ierr);
                    voff[0] = vertices[0] - vertexstart;
                    voff[1] = vertices[1] - vertexstart;
                    x = 0.5 * (acoords[2*voff[0]+0] + acoords[2*voff[1]+0]);
                    y = 0.5 * (acoords[2*voff[0]+1] + acoords[2*voff[1]+1]);
                    ierr = PetscSynchronizedPrintf(comm,
                        "    edge   %3d at (%g,%g)\n",pts[p],x,y); CHKERRQ(ierr);
                }
            }
        }
        ierr = DMPlexRestoreTransitiveClosure(plex, j, PETSC_TRUE, &numpts, &pts); CHKERRQ(ierr);
    }
    ierr = VecRestoreArray(coords, &acoords); CHKERRQ(ierr);
    ierr = PetscSynchronizedFlush(comm,PETSC_STDOUT); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode VecViewLocalStdout(Vec v, MPI_Comm gcomm) {
    PetscErrorCode ierr;
    int         m,locsize;
    PetscMPIInt rank,size;
    double      *av;
    const char  *vecname;
    ierr = MPI_Comm_size(gcomm,&size);CHKERRQ(ierr);
    ierr = MPI_Comm_rank(gcomm,&rank); CHKERRQ(ierr);
    ierr = PetscObjectGetName((PetscObject)v,&vecname); CHKERRQ(ierr);
    ierr = PetscPrintf(gcomm,"local Vec: %s %d MPI processes\n",
                       vecname,size); CHKERRQ(ierr);
    if (size > 1) {
        ierr = PetscSynchronizedPrintf(gcomm,"[rank %d]:\n",rank); CHKERRQ(ierr);
    }
    ierr = VecGetLocalSize(v,&locsize); CHKERRQ(ierr);
    ierr = VecGetArray(v, &av); CHKERRQ(ierr);
    for (m = 0; m < locsize; m++) {
        ierr = PetscSynchronizedPrintf(gcomm,"%g\n",av[m]); CHKERRQ(ierr);
    }
    ierr = VecRestoreArray(v, &av); CHKERRQ(ierr);
    ierr = PetscSynchronizedFlush(gcomm,PETSC_STDOUT); CHKERRQ(ierr);
    return 0;
}