1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 #include <cmath>
20 #include "SphereNEDSSphereNEDSR.hpp"
21 #include <Interaction.hpp>
22 #include <BlockVector.hpp>
23
SphereNEDSSphereNEDSR(double r,double rr)24 SphereNEDSSphereNEDSR::SphereNEDSSphereNEDSR(double r,
25 double rr)
26 : NewtonEuler3DR()
27 {
28 r1 = r;
29 r2 = rr;
30 r1pr2 = r1 + r2;
31 }
32
distance(double x1,double y1,double z1,double r1,double x2,double y2,double z2,double r2)33 double SphereNEDSSphereNEDSR::distance(double x1, double y1, double z1, double r1,
34 double x2, double y2, double z2, double r2)
35 {
36 double dx = x1 - x2;
37 double dy = y1 - y2;
38 double dz = z1 - z2;
39
40 return (sqrt(dx * dx + dy * dy + dz * dz) - r1pr2);
41 }
42
43
computeh(double time,const BlockVector & q0,SiconosVector & y)44 void SphereNEDSSphereNEDSR::computeh(double time, const BlockVector& q0, SiconosVector& y)
45 {
46
47
48 double q_0 = q0(0);
49 double q_1 = q0(1);
50 double q_2 = q0(2);
51 double q_7 = q0(7);
52 double q_8 = q0(8);
53 double q_9 = q0(9);
54
55 y.setValue(0, distance(q_0, q_1, q_2, r1, q_7, q_8, q_9, r2));
56 //Approximation _Pc1=_Pc2
57 _Pc1->setValue(0, (r1 * q_0 + r2 * q_7) / (r1 + r2));
58 _Pc1->setValue(1, (r1 * q_1 + r2 * q_8) / (r1 + r2));
59 _Pc1->setValue(2, (r1 * q_2 + r2 * q_9) / (r1 + r2));
60 _Pc2->setValue(0, (r1 * q_0 + r2 * q_7) / (r1 + r2));
61 _Pc2->setValue(1, (r1 * q_1 + r2 * q_8) / (r1 + r2));
62 _Pc2->setValue(2, (r1 * q_2 + r2 * q_9) / (r1 + r2));
63 _Nc->setValue(0, (q_0 - q_7) / (y.getValue(0) + r1pr2));
64 _Nc->setValue(1, (q_1 - q_8) / (y.getValue(0) + r1pr2));
65 _Nc->setValue(2, (q_2 - q_9) / (y.getValue(0) + r1pr2));
66 //std::cout<<" SphereNEDSSphereNEDSR::computeh dist:"<<y->getValue(0)<<"\n";
67 //std::cout<<"_Pc:\n";
68 //_Pc->display();
69 //std::cout<<"_Nc:\n";
70 //_Nc->display();
71 }
72