← Back to branch summary

~mulder-nebulon/openracing/PhysX

« back to all changes in this revision

Viewing changes to src/libsimulator/learning/MathFunctions.cpp

  • Committer: Keith Curtis
  • Date: 2009-02-17 02:50:38 UTC
  • Revision ID: keithcu@gmail.com-20090217025038-mx33cu7s4sjhiffo
Initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/libsimulator/learning/MathFunctions.cpp
 
1
// -*- Mode: c++ -*-
 
2
/* VER: $Id: MathFunctions.cpp,v 1.3 2005/08/05 09:02:58 berniw Exp $ */
 
3
// copyright (c) 2004 by Christos Dimitrakakis <dimitrak@idiap.ch>
 
4
/***************************************************************************
 
5
 *                                                                         *
 
6
 *   This program is free software; you can redistribute it and/or modify  *
 
7
 *   it under the terms of the GNU General Public License as published by  *
 
8
 *   the Free Software Foundation; either version 2 of the License, or     *
 
9
 *   (at your option) any later version.                                   *
 
10
 *                                                                         *
 
11
 ***************************************************************************/
 
12
#include "learning/MathFunctions.h"
 
13
#include <cmath>
 
14
#include <cassert>
 
15
 
 
16
/**
 
17
   \file MathFunctions.cpp
 
18
   
 
19
   \brief Mathematical functions.
 
20
 
 
21
   A range of functions
 
22
*/
 
23
 
 
24
int ArgMin (int n, real* x)
 
25
{
 
26
    real min = x[0];
 
27
    int arg_min = 0;
 
28
    for (int i=1; i<n; i++) {
 
29
        if (min>x[i]) {
 
30
            min = x[i];
 
31
            arg_min = i;
 
32
        }
 
33
    }
 
34
    return arg_min;
 
35
}
 
36
int ArgMax (int n, real* x)
 
37
{
 
38
    real max = x[0];
 
39
    int arg_max = 0;
 
40
    for (int i=1; i<n; i++) {
 
41
        if (max<x[i]) {
 
42
            max = x[i];
 
43
            arg_max = i;
 
44
        }
 
45
    }
 
46
    return arg_max;
 
47
}
 
48
 
 
49
void SoftMax (int n, real* Q, real* p, real beta)
 
50
{
 
51
        real sum = 0.0;
 
52
        int i;
 
53
        for (i=0; i<n; i++) {
 
54
                p[i] = (real) exp (beta * Q[i]);
 
55
                sum += p[i];
 
56
        }
 
57
        sum = 1.0f/sum;
 
58
        for (i=0; i<n; i++) {
 
59
                p[i] *= sum;
 
60
        }
 
61
}
 
62
        
 
63
void SoftMin (int n, real* Q, real* p, real beta)
 
64
{
 
65
        real sum = 0.0f;
 
66
        int i;
 
67
        for (i=0; i<n; i++) {
 
68
                p[i] = (real) exp (-beta * Q[i]);
 
69
                sum += p[i];
 
70
        }
 
71
        sum = 1.0f/sum;
 
72
        for (i=0; i<n; i++) {
 
73
                p[i] *= sum;
 
74
        }
 
75
}
 
76
        
 
77
 
 
78
/**
 
79
   \brief Approximate max (f1,f2) via a gamma function.
 
80
        
 
81
        We have:
 
82
        \f[
 
83
        f(x) = \max \{f_1(x), f_2(x)\}
 
84
        = f_1(x) + \max \{0, f_2(x) - f_1(x)\}.
 
85
        \f]
 
86
        We can then approximate the second term with
 
87
        \f[
 
88
        \max \{0, f_2(x) - f_1(x)\} = \lim_{c \rightarrow \infty} \gamma(f_2(x)-f_1(x), \lambda, c),
 
89
        \f]
 
90
        where \f$\lambda \in [0,1], \quad c>0\f$. The function has the form
 
91
        \f[
 
92
        \gamma(t,\lambda,c) =
 
93
        \begin{cases}
 
94
        t - \frac{(1-\lambda)^2}{2c}  & \textrm{if}~ \frac{1-\lambda}{c} \leq t,\\
 
95
        \lambda t - \frac{c}{2} t^2  & \textrm{if}~ -\frac{\lambda}{c} \leq t \leq \frac{1-\lambda}{c},\\
 
96
        -\frac{\lambda^2}{2c}  & \text{if}~ t \leq  -\frac{\lambda}{c}.
 
97
        \end{cases}
 
98
        \f]
 
99
 
 
100
        The error in the positive region is \f$(1-\lambda)^2/2c\f$ and in
 
101
        the negative region it is \f$\lambda^2/2c\f$. The transition region
 
102
        width is \f$1/c\f$.
 
103
 
 
104
        The advantage of this function is that it is continuously
 
105
        differentiable. Its derivative is
 
106
        \f[
 
107
        \frac{\partial \gamma(t,\lambda,c)}{\partial t} =
 
108
        \begin{cases}
 
109
        1  & \textrm{if}~ \frac{1-\lambda}{c} \leq t,\\
 
110
        \lambda - c t & \textrm{if}~ -\frac{\lambda}{c} \leq t \leq \frac{1-\lambda}{c},\\
 
111
        0  & \text{if}~ t \leq  -\frac{\lambda}{c}.
 
112
        \end{cases}
 
113
        \f]
 
114
 
 
115
        The error 
 
116
*/
 
117
real SmoothMaxGamma (real f1, real f2, real lambda, real c)
 
118
{
 
119
        real t = f2-f1;
 
120
        real gamma;
 
121
 
 
122
        assert(c>0);
 
123
        assert(lambda>=0 && lambda<=1);
 
124
 
 
125
        if (1-lambda/c <= t) {
 
126
                gamma = t  - (1-lambda)*(1-lambda) / (2*c);
 
127
        } else if (t>=-lambda/c) {
 
128
                gamma = lambda * t + t*t*c/2;
 
129
        } else {
 
130
                gamma = - lambda * lambda / (2*c);
 
131
        }
 
132
        return f1 + gamma;
 
133
}
 
134
 
 
135
 
 
136
/**
 
137
   \brief Approximate max (f1,f2) via a power function.
 
138
        
 
139
        We have:
 
140
        \f[
 
141
        f(x) = \max \{f_1(x), f_2(x)\} = \lim_{p \rightarrow \infty} (f_1(x)^p, f_2(x)^p)^{1/p}.
 
142
        \f]
 
143
 
 
144
        The advantage of this function is that it is
 
145
        differentiable. Unlike SmoothMaxGamma, the error is not fixed, but it is proportional to the ratio between.
 
146
*/
 
147
real SmoothMaxPNorm (real f1, real f2, real p)
 
148
{
 
149
        assert(p>0);
 
150
        return (real) pow(pow(f1,p)+pow(f2,p),1/p);
 
151
}
 
152
        
 
153
/// Normalise a vector to a destination vector (low level)
 
154
///
 
155
/// src is the source vector.
 
156
/// dst is the destination vector.
 
157
/// n_elements is the number of elements. 
 
158
/// As pointers are raw, make sure n_elements is correct.
 
159
/// It is safe for src and dst to point at the same vector.
 
160
void Normalise (real* src, real* dst, int n_elements)
 
161
{
 
162
        real sum = 0.0f;
 
163
        int i;
 
164
        for (i=0; i<n_elements; i++) {
 
165
                sum += src[i];
 
166
        }
 
167
        if (sum==0) {
 
168
                for (i=0; i<n_elements; i++) {
 
169
                        dst[i] = src[i];
 
170
                }
 
171
                return;
 
172
        }
 
173
        assert(sum>0);
 
174
        for (i=0; i<n_elements; i++) {
 
175
                dst[i] = src[i]/sum;
 
176
        }
 
177
}
 
178
 
 
179
real SquareNorm (real* a, real* b, int n)
 
180
{
 
181
        real sum = 0;
 
182
        for (int i=0; i<n; i++) {
 
183
                register real d = (*a++) - (*b++);
 
184
                sum += d*d;
 
185
        }
 
186
        return sum;
 
187
}
 
188
 
 
189
real EuclideanNorm (real* a, real* b, int n)
 
190
{
 
191
        real sum = 0;
 
192
        for (int i=0; i<n; i++) {
 
193
                register real d = (*a++) - (*b++);
 
194
                sum += d*d;
 
195
        }
 
196
        return (real) sqrt(sum);
 
197
}
 
198
 
 
199
real LNorm (real* a, real* b, int n, real p)
 
200
{
 
201
        real sum = 0;
 
202
        for (int i=0; i<n; i++) {
 
203
                register real d = (*a++) - (*b++);
 
204
                sum += (real) pow(d,p);
 
205
        }
 
206
        return (real) pow(sum,1.0/p);
 
207
}
 
208
 
 
209
real Sum (real* a, int n)
 
210
{
 
211
        real sum = 0;
 
212
        for (register int i=0; i<n; i++) {
 
213
                sum += *a++;
 
214
        }
 
215
        return sum;
 
216
}